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1.0  Introduction 


Under  contract  F33615-89-C-0574  Task  08 A  "Human  Sensory  Feedback",  Systems 
Research  Laboratories  provided  engineering  and  scientific  support  to  explore  the  utility 
of  telerobotic  systems  m  hazardous  environments.  A  testbed  was  developed  at  the  USAF 
Armstrong  Laboratory's  Human  Sensory  Feedback  Lab  at  Wright  Patterson  AFB,  to 
evaluate  and  research  issues  in  course  manipulation,  bilateral  teleoperation,  force-control 
and  man-machine  interfaces. 


2.0  Technical  Description 

The  facility's  testbed  provides  control  of  wrist  position  and  end  effector  orientation  for 
one  six  degree-of-fre^om  (DOF)  American  Cimflex  Merlin  industrial  robot  arm  using  a 
MB  Associates  7  DOF  exoskeleton  worn  by  a  human  operator.  A  Compaq  386  33  MHz 
computer  serves  as  the  host  controller  for  the  following  functions: 

Computation  of  Cartesian  position  and  orientation  for  various  coordinate  frames 
assigned  at  fixed  locations  on  the  exoskeleton,  using  joint  angles  received  from 
the  exoskeleton  over  a  hardwire  serial  link  at  38.4k  bits/ sec. 

Computation  of  joint  angles  required  for  the  Merlin  robot  to  achieve  a  desired 
wrist  position  and  end  effector  orientation  driven  by  the  exoskeleton  as  a  master. 

Provide  user  with  system  control  functions. 


3.0  MBAssociates  Unilateral  Exoskeleton 

The  MBAssociates  exoskeleton  was  originally  constructed  by  MBAssociates  of  San 
Ramon,  CA  under  contract  No.  F08635-75-C-0027  with  the  United  States  Air  Force, 
Armament  Development  and  Test  Center  at  Eglin  AFB,  Florida.  The  exoskeleton  was 
part  of  a  Manipulator  Arms  System  comprised  of  an  anthropomorphic  two-armed 
teleoperator  slave,  powered  by  an  electronically  controlled  hydraulic  system  and 
controlled  by  a  two-armed  exoskeletal  type  master.  The  master's  grip  and  elbow  joints 
ron  both  arms  provided  force  feedback  by  use  of  local  hydraulic  valves. 

The  Human  Sensory  Feedback  Lab  accmired  the  MBA  exoskeleton  through  a  long-term 
equipment  loan  arrrangement  with  the  Department  of  Energy's  Oak  Ridge  National 
Laboratories. 

Several  modifications  were  performed  on  the  MBA  exoskeleton  to  enhance  its  use  as  a 
standalone  device  in  the  Human  Sensory  Feedback  Testbed. 
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Since  initial  use  of  the  MBA  exoskeleton  was  planned  for  position  cpntrol  of  the  Merlin 
robot,  the  exoskeleton's  force  feedback  hydraulic  actuators  and  manifold  system  were 
removed  to  reduce  operator  fatigue  due  to  the  effects  of  gravity.  A  microprocessor 
bas^  computer,  residing  in  the  exoskeleton's  baclqiack,  was  developed  to  read  joint 
angles  from  either  arm,  using  optical  encoders  located  at  each  arm's  7  joints  plus  gripper 
control,  and  send  them  over  a  serial  hardwire  link  to  a  host  computer  serving  as  the 
central  controller  for  the  exoskeleton  and  robot. 


3.1  Backpack  Computer  Description 

To  provide  the  potential  for  the  exoskeleton  to  be  operated  as  a  standalone  device  in  a 
location  remote  from  the  Merlin  robot,  a  68000  microprocessor  based  computer  with  2 
asynchronous  serial  ports,  battery  backed  SRAM  and  incremental  optical  encoder 
interface  circuitry  was  developed  to  reside  in  the  exoskeleton's  baclmack.  Exoskeleton 
arm  linkage  joint  angles,  required  to  compute  Cartesian  position  and  orientation  for 
exoskeleton  coordinate  frames,  can  be  requested  and  received  from  a  remote  host 
controller  over  a  38.4K  bits/sec  hardwire  serial  link. 

Joint  angles  are  sensed  by  incremental  optical  encoders  with  1024  counts/rev  for  1:1 
ratio  joints  and  120  counts/rev  for  gear/belt  joints.  Incremental  (mtical  encoder 
interface  circuitry  effectively  increases  the  counts/rev  to  4096  and 480  respectively,  by 
counting  the  rising  and  falling  edges  of  both  quadrature  channels  from  a  single  encoder. 
Incremental  type  encoders  were  selected  over  the  original  potentiometers  to  increase  joint 
angle  resolution  and  reduce  suseptablity  to  electrical  interference.  Incremental  type 
encoders  were  chosen  over  absolute  types  for  cost  considerations  with  the  tradeoff  being 
the  need  to  initialize  each  joint  of  the  exoskeleton  after  loss  of  power. 

Serial  communications  between  the  MBAssociates  exoskeleton  and  an  external  computer 
are  specified  electrically  as  RS-422  differential  type  drivers  and  receivers  for  longer 
distance  and  higher  noise  immunity  reasons.  The  default  bits/sec  rate  is  38. 4K  with  1 
Meg  bits/sec  possible  by  selecting  a  higher  oscillator  base  frequency  for  the  DU  ART 
serial  communications  chip. 


3.2  Backpack  Software  Description 


The  68000  microprocessor  executable  code  is  downloaded  from  the  host  controller  via 
the  hardwire  serial  link  to  the  MBA  exoskeleton  backpack  computer.  This  provides 
development  flexiblity  in  changing  the  software  that  initializes  the  incremental  encoder 
interface  circuitry,  reads  exoskeleton  joint  angle  encoders  and  grijmer  switches  on 
command,  and  communicates  in  real  time  with  a  host  controller.  Upon  exoskeleton 
power-on/reset,  a  downloader  routine,  resident  in  EPROM  (firmware)  on  the  backpack's 
computer  board,  monitors  the  RS-422  serial  port  for  a  download  command  from  the  host 
computer  and  handles  the  transfer  of  68(X)0  executable  code  to  an  area  in  battery-backed 
SRi^.  Once  the  download  processes  conmlete,  the  on-board  68000  CPU  exits  the 
downloader  routine  and  begins  execution  of  the  downloaded  code  in  SRAM. 

The  downloader  software  was  developed  on  a  Hewlett-Packard  64000  Development 
System  with  a  68000  microprocessor  emulation  pod.  The  emulation  pod  plugs  directly 
into  the  68000  microprocessor  socket  on  the  baclqpack  computer  board  to  allow 
development  and  testing  of  code  on  the  target  hardware,  with  finalized  downloader  code 
programmed  into  EPROM.  All  downloader  software  is  archived  on  a  Hewlett-Packard 
9134  hard  disk  drive  associated  with  the  HP  64000  Development  System. 

Development  of  a  baclmack  computer  program  to  be  downloaded  is  initiated  using 
Aztec_C  high  level  ”C*  programming  language  with  Borland  Turbo  C  II  as  a 
development  tool  on  a  Compaq  386  PC.  Using  the  Aztec  C  cross-compiler  and  linker, 
68000  executable  code  is  produced  for  downloading  to  the  MBA  exoskeleton  backpack 
computer. 

The  downloading  of  68000  executable  code  from  the  host  Compaq  386  PC  to  the  MBA 
exoskeleton  bactoack  computer  is  handled  by  routines  c^led  by  the  program  merlin.exe 
on  the  Compaq  386. 
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3.3  Forward  Kinematics  for  MBAssociates  exoskeleton 

Forward  kinematic  transformation  matrices  were  developed  using  the  Denavit- 
Hartenberg  convention  as  described  in  Reference[l]. 

General  form  of  jT  : 


C0j 

-S0j 

0 

ai-1 

i-1  »T'  ^ 

i  ^ 

s0jCaj_j 

c0jCaj.j 

-saj.i 

-satj.jdj 

s0jSaj.i 

c0jSaj.j 

caj.i 

cai.jdj 

0 

0 

0 

1 

Where: 

aj  =  the  distance  from  Zj  to  j  measured  along  Xj 
aj  =  the  angle  between  Zj  and  Zj+j  measured  along  Xj 
dj  =  the  distance  from  Xj.j  to  Xj  measured  along  Zj 
0j  =  the  angle  between  Xj.  j  and  Xj  measured  about  Zj 
c  =  cosine 

s  =  sine 


Location  of  Frame  {i}'s  Zj  axis  was  chosen  to  be  along  the  optical  encoder  output  shaft 
of  joint  {j}  The  direction  of  Xj  was  chosen  to  insure  that  the  value  of  0j 
increases/decreases  in  the  same  direction  as  the  encoder  counts.  Encoder  counts  increase 
with  clockwise  rotation  of  the  shaft  as  viewed  from  the  encoder  body  out  towards  the 
shaft. 


Frame  assignments  and  matrices  are  shown  for  each  joint  of  each  arm  to  notate  the 
sparse  composition  of  most  matrices  and  to  relation  program  execution  times  for 
kinematic  computations. 
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3.3.1  Left  Ann 


Reference  Frame  lOl 

The  Origin  of  Reference  Frame  {0}  is  at  the  countersunk  screw  located  top  center  on  the 
MBA's  backplate.  (See  figure  1) 


Xo 


Zo 


ao  =  0 

«o  =  0° 

do  =  Does  not  exist 
Gq  =  Does  not  exist 


T^ft  Frame  111 

ai  =  +I3 

cKj  =  +189° 

di  =  (li  +  I2  tan  9°)/tan  9° 
01  =0° 


Zj  YZ  View 
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To  solve  for  dj, 

di  =  ( li  +  I2  tan  9®)/  tan  9° 

where  :  (See  Figure  1) 

1,  =  6.9375  distance  from  Zq  in  +Yq  direction 
=  5.3075  distance  from  Zq  in  +Zn  direction 
1,  =  5.5156  distance  from  back  plate  to  Z2  in  H-Xn  direction 
l]  =  4.9375 
I5  =  12.141 


1 

0 

0 

0 

II 

H 

0 

0 

1 

0 

0 

0 

0 

1 

(Ij  +  I2  tan  9®)  /  tan  9® 

0 

0 

0 

1 

T.i»ft  Frame 

Located  at  Shoulder  Azimuth  and  Shoulder  Elevation  Axes 
point  of  intersection. 


82  =  0 
a  2  =  -90» 

d2  =  (li  +  I4  sin  9'’)/sin  9° 
©2  =  Shoulder  Azimuth  Joint 
+90°  all  the  way  back 
+225.9°  all  the  way  forward 


002 

-se2(.988) 

-se2(.156) 

0 


-S02  0 

-c02(.988)  .156 

-c02(.156)  -.988 

0  0 


.156(12 -l-l4sm9‘’)/sin9° 
-.988(lj+l4sm9°)/sin9 
1 


7 


Frame 

Located  at  the  shoulder  Elevation  &  Upper  Arm  Roll  Intersect. 


Z3 


83—0 

a  3  =  -90° 

d3  =  -.0938 

©3  =  Shoulder  Elevation 

-1-178°  all  the  way  back 
+46.4°  all  the  way  forward 


C0q 


-S0O 


-S0^ 


0 

-C03 
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0 

1 

0 

0 


-.0938 


0 
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Left  Frame  {4} 

Located  at  upper  arm  roll  and  elbow  intersect. 


z 
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X4  Z4 


a4  =  0 
a4  =  -90° 

44  =  I5 

©4  =  Upper  arm  Roll 

-43.9°  CW  Hard  Stop 
173.7°  CCW  Hard  Stop 
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T  = 
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1 
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T^ft  Frame  (S\) 

Located  at  Elbow  and  Lower  Arm  Roll  Intersect 


85  =  0 
«5  =  -90° 
d5  =  -.313 
65  =  Elbow  Joint 

-1-46°  All  the  way  back 
-1-316. 1°  All  the  way  up 


4 
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T  = 


065  -805  0 

0  0  1 

-S0J  -C0J  0 

0  0  0 


Frame  {6} 

Located  at  Lower  Arm  Roll  and  Wrist  Radial  Intersect 


0 

-.313 

0 
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X6 


ag  =  0 

(.6  =  90° 

dg  =  l6  =  11.250 

06  =  Lower  Arm  Roll  Joint 


0 

1 

0 

0 


0 

k 

0 
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Left  Frame 

Located  at  Wrist  Radial  and  Wrist  Flex  Intersect 
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Left  Frame  181 

Located  at  Wrist  Flex 
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=  Not  Defined 
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=  Wrist  Flex 
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3.3.2  Right  Arm 


The  Origin  of  Reference  Frame  {0}  is  at  the  countersunk  screw  located  top  center  on  the 
MBA's  backplate.  (See  figure  1) 


=  +171° 

di  =  (li  +  I2  tan  9°)/tan  9° 
01  =  0° 


Ij  +  I2  tan  9° 
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To  solve  for  dj, 

di  =  ( li  +  I2  tan  9°)/  tan  9° 

where: 

1,  =  6.9375  distance  from  Zq  in  -Yq  direction 
I2  =  5.3075  distance  in  -Zq  airection  from  Zq 
n  =  5.5156  distance  in  direction  from  back  plate  to  Z, 

1^  =  4.9375  ° 

I5  =  12.141 

10  0  0 
T  =  0  10  0 

0  0  1  (Ij  +  I2  tan  9*)  /  tan  9° 

0  0  0  1 


Located  at  Shoulder  Azimuth  and  Shoulder  Elevation  Axes 
point  of  intersection. 


Z2 


32=0 

a2  =  90° 

d2  =  O1+I4  sin  9°)/sin9® 

©2  =  Shoulder  Azimuth  Joint 
+270°  all  the  way  back 


+  134.1°  all  the  way  forward 


C02 

-s02(.988) 

-s02(.156) 

0 


-S02  0  +1^ 

-c02(.988)  .156  . 15602 +I4sin9‘’)/sin9° 

-c02(.156)  -.988  -.9880i+l4sin9'’)/sin9° 

0  0  I 
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Wight  Frame  {3} 

Located  at  the  shoulder  Elevation  &  Upper  Arm  Roll  Intersect. 


as  =  0 

a3=-90° 
d  3  =  -.032 

©3  =  Shoulder  Elevation 

-1-2°  back 

-t- 133.6°  forward 


2 

3 


T  = 


C0O 


-S0.J 


-S0q 


C0q 


0 

-1 

0 

0 


0 

-.032 

0 

1 


Right  Arm  Frame  {41 


Located  at  upper  arm  roll  and  elbow  intersect. 


34  =  0 
04  =  90° 

^4  =  ^5 

©4  =  Upper  arm  Roll 
0°  Rotated  Inward 
217.6°  Rotated  Outward 
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0 


0 


C04  -S04 

0  0  1% 
^4T=  -S04  -C04  0  0 

0  0  0  1 


Right  Frame  f5l 

Located  at  Elbow  and  Lower  Arm  Roll  Intersect 

35=0 

«5  =  90° 
d5  =  0 

05  =  Elbow  Joint 

+46°  All  the  way  up 
+316.1°  All  the  way  back 

-S05  0  0 

0-10 
C05  0  0 

0  0  1 
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Right  Arm  Frame  I6> 

Located  at  Lower  Arm  Roll  and  Wrist  Radial  Intersect 


^6 


ag  =  0 

a  6  =  -900 
d6  =  l6  =  11-330 

=  Lower  Arm  Roll  Joint 
Range  of  Motion; 

-97.32°  Inward 
119.91°  Outward 


C0/; 


-S0A 


0 

S06 

0 


C0A 


0 

0 


0 

1 


Frame 

Located  at  Wrist  Radial  and  Wrist  Flex  Intersect 


X7 


Si'j  =  0 
aj  =  -90° 

dj  =  -.160 

©7  =  Wrist  Radial 
73.84°  Up 
105.57°  Down 


cG-y  -sG-y  0  0 

0  0  1  -.160 

-sOj  -cGy  0  0 

0  0  0  1 


a  g  =  Not  Defined 
a  g  =  Not  Defined 
d  8  =  0 

0g  =  Wrist  Flex 
-47.35°  Outward 
42.74°  Inward 


cGg  ^  ^ 

0  0  10 

^g  T  =  "®®8  '^^8  ^  ^ 

0  0  0  1 
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4.0  Inverse  Kinematics  for  Merlin  Robot 

The  position  of  Merlin's  wrist  (coincident  origins  of  frames  {4} {5} {6} )  with  respect  to 
Merlin's  fixed  reference  frame  {0}  is  driven  by  the  position  of  MBA  frame  {6}  origin 
(wrist  radial)  with  respect  to  MBA's  fixed  reference  frame  {0}. 

Orientation  of  Merlin's  wrist  (tool  roll  frame  {6})  with  respect  to  Merlin's  fixed 
reference  frame  {0}  driven  by  the  orientation  of  the  MBA  exoskeleton's  wrist  (wrist  flex 
frame  {8}  with  respect  to  the  MBA's  exoskeleton's  fixed  reference  frame  {0}. 

4.1  Approach 

Merlin's  wrist  position  determined  by  driving  Merlin's  frames  {1}  (waist),  (2) 
(shoulder),  and  {3}  (elbow)  directly  from  the  results  of  the  following  inverse  Kinematics. 

Utilize  Z-Y-Z  Euler  angle  convention  to  describe  Merlin's  wrist  orientations.  Z-Y-Z 
Euler  angles  with  respect  to  Merlin's  elbow  frame  {3}  can  be  used  directly  to  drive 
Merlin's  wrist  roll,  wrist  flex  &  tool  roll  revolute  joints  64,  65,  0^.  respectively. 
Derivation  of  Z-Y-Z  Euler  angles  to  describe  the  MBA's  wrist  (wnst  flex  frame  {8}) 
string  with  MBA  {8}  coinciaent  with  MBA  elbow  frame  {5}  will  result  in  an 
orientation  of  the  Merlin  wrist  (6}  different  from  the  MBA  wrist  {8}.  Therefore,  a 

ghantom  frame  {A}  was  definea  on  the  MBA  with  an  origin  coincident  at  MBA  {6} . 
low  the  orientation  of  MBA  frame  {8}  wrist  flex  can  be  described  by  Z-Y-Z  Euler 
angles  from  frame  {A},  thereby  yielding  results  that  can  be  used  directly  to  drive 
Merlin's  ©4  65  0^  to  orient  Merlin's  wrist. 
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MRA  Hxoskeleton  Merlin  Robot 


{51  =  elbow 
lAl  = 

{3}  =  elbow 

{8)  =  wrist  flex 

Want : 

Global  0 

0^ 

= 

Global  T> 

0^ 

Know  : 

O5R 

O3R 

Need  : 

= 

O3R 

Solve  for: 

V  =  V 

O3R 

= 

O3R 

Need  : 

A  p 

Know  : 

\R  &  \R 

So  : 

a,r  =  AjR 

Solve  for: 

DO 

II 

Then: 

^11 

'‘12 

^13  1 

> 

00 

DO 

II 

hi 

>■22 

■■23  1 

^  z  y  z 

hi 

*■32 

*■33  1 

B  =  Atan2  (v/"  +  r  32^)  ,  r33)  =  wrist  flex  (04) 

a  =  Atan2  (  r23  /  sin  6  ,  rj3  /  sin  fi  )  =  wrist  roll  (65) 
T  =  Atan2  (  r32  /  sin  6  ,  -r3j  /  sin  6 )  =  tool  roll  (Gg) 
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4.2  High  Speed  Host  Interface 

The  High  Speed  Host  Interface  developed  by  American  Cimflex  provides  a  parallel 
shared  memo^  interface  between  the  Compaq  386  host  controller  computer  and  the 
Merlin  controller's  master  CPU.  The  HSHI  software,  executing  on  the  Merlin 
controller's  master  CPU  board,  performs  motion  related  commands  and  provides  status 
for  each  robot  joint. 


4.3  Hardware  and  Configuration 

The  American  Cimflex  MR6500  Merlin  robot  is  equipped  with  stepper  type  motors  for 
each  of  the  six  joints.  The  Waist,  Shoulder,  and  Elbow  joints  are  actuated  with  Sigma 
802-D42-802-8/80  steppers,  and  the  three  wrist  joints  are  actuated  by  Sigma  802- 
D28698  steppers.  Sigma  Motion  Control  Products  was  bought  out  by  Pacific  Scientific 
in  Rockford,  IL  815-226-3100. 

The  Bit3  PC/AT  to  VME  link  is  configured  with  Dual  Port  Ram  (DPRl  loacated  at 
address  lOOOOOH  on  VME  board.  Jumper  diagrams  apprear  in  Appenmx  C.  Note:  PC 
Memory  extension  software  drivers  such  as  EMM386  may  not  be  compatible  with 
PC  side  of  Bit3.  (See  Reference  [6]  Bit3  IBM  PC/AT  VME  Adaptor  Model  403 
Manual) 

HAL  Engineering  VME  to  Versabus  Adapter  Card  installed  in  Merlin  Control  Cabinet 
Versabus.  Bit3  VME  card  is  installed  into  Adapter  card. 

HSHI  Eproms  (2716  2Kx8)  must  be  installed  for  each  axis  on  Merlin's  Axis  Control 
Boards.  (See  Reference  [4]  HSHI  Manual) 


Editing  a  HSHI  Executive  System  Disk 
(See  Reference  [4]  ARBASIC  Manual) 

1)  MAGIX  SYSTEMS  DISK  in  Merlin  Controller's  DRIVE  0 

2)  AR-BASIC  in  DRIVE  1 

3)  Front  Panel  key  switch  in  "PROG"  position 

4)  Power  on  Merlin  Controller  Circuit  Breaker 

5)  Depress  Front  Panel  "Power  On"  button 
61  MAGIX  OS  and  AR-BASIC  will  auto-load 

7)  If  Front  Panel  "CAL"  button  blinks.  Merlin's  calibration  procedure  must  be 
performed  by  engaging  the  motors  and  pressing  the  Front  Panel  "CAL"  button. 
Merlin  willjostle  all  joints  and  return  AR-BAmC  prompt "  < "  on  the  CRT. 

8)  With  the  AR-BASIC  prompt "  < "  on  the  CRT,  replace  the  AR-BASIC  disk  with 
the  HSHI  disk  to  be  edited. 
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To  tell  MAGIX  OS  to  track  disk  changes, 
Type  "LOAD  /usr/etc/sysexec.def.  The 
wifi  appear  on  the  CRT'^s  top  window. 

To  edit  the  file  for  the  HSHI  Menu  Option: 

-  Press  "FI"  to  enter  CRT  edit  window 

-  Edit  the  file  as  follows: 


type  "F  LIST  /etc/" 

HSHI  Ececutive  System  file  contents 


!  boot  HSHI  initialW  ... 

!  Echo  Booting  HSHI 
!  hshi  -m  100000 
!  Kill  JOAN 
!  Host  mode 

12)  To  edit  the  file  for  auto-boot  of  HSHI  Host  mode  @  power-on 
! 

I  boot  HSHI  initialW  ... 

!  Echo  Booting  HSHI 
hshi  -m  100000 
Kill  JOAN 
!  Host  mode 

13)  Press  "FI"  to  return  to  AR-BASIC's  command  line  on  CRT 

14)  Type  "SAVE  /usr/etc/sysexec.def 

15)  T^  "M  DELETE  $pgm"  to  remove  file  from  editor  memory 

16)  Type  "BYE"  to  exit  AR-BASIC 


4.4  Troubleshooting 
Merlin  Robot 

Sympton  -  Joint  drifts/  non  responsive 

Correction  -  If  green  LED  on  motor  controller  card  is  not  "on",  replace/repair 
motor  controller  card. 

-  If  green  LED  on  motor  controller  card  is 
"on",  remate  all  motor  and  encoder  connectors  in  junction  box 
at  robot  base. 


Intecolor  2400  Series  Terminal 

Refer  to  "Maintenance  Manual  for  Intecolor  2400  Series 
Terminals  with  ColorTrend  220  Supplement" 
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5.0  Host  Controller  Compaq  386  33MHz 

5.1  Hardware  and  Configuration 

(See  Figure  2) 

A  Compaq  386  33  MHz  desktop  PC  serves  as  the  system  host  controller,  software 
development  computer  and  operator  interface  to  the  testbed. 

The  MBA  exoskeleton  is  linked  to  the  system  host  via  a  38.4  K  bits/sec  RS-422 
asynchronous  serial  link. 

The  Merlin  robot  controller  is  linked  to  the  system  host  through  a  parallel  link  using  dual 
port  ram  provided  by  a  Bit  3  Model  403  PC/AT-VME  Adaptor. 

The  JR3  Univeral  Force/Moment  System  is  linked  to  the  system  host  controller  via  a 
9^  bits/sec  RS-232  asynchronous  serial  link. 


5.2  MBA/Merlin  control  program 

Program  execution  times  for  merlin.exe  on  a  Compaq  386  33  Mhz  computer  with 
floating  point  numeric  coprocessor: 

MBA  exoskeleton's  forward  kinematic  algorithm  0.732  mS 

Merlin  robot  inverse  kinematic  algorithm  0.960  mS 

Serial  transmission  time  to  transfer 
7  joint  angle  readings  from  exoskeleton  right  arm. 

This  time  is  transparent  to  total  control 
loop  time  as  it  is  interrupt  driven. 

7  joints  *  2  integer  bytes/joint  * 

10  bits/integer  value  transmitted  * 

38.4  Kbits/ sec  (3.64  mS) 

Time  for  serial  interrupt  servicing, 
pre-processing  of  joint  angle  readings 
for  forward  kinematics,  limit  and  error 
checking  on  inverse  kinematic  results  and 

operator  display  updates.  8.18  mS 

Total  control  loop  time  9.87  mS 


The  9.87  mS  (101  Hz)  total  control  loop  time  is  greater  than  the  Merlin's  4  mS  (250  Hz) 
servo  loop  time  achievable  under  optimal  HSHI  operation.  This  difference  in  loop  times 
means  the  Merlin  is  not  receiving  motor  encoder  servo  values  fast 
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MBA/MERLIN  TESTBED  BLOCK  DIAGRAM 


L 

3 

CD 

Li- 


i 
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SYSTEM 


enough  from  the  host  controller.  Better  real  time  response  in  positioning  motions  may 
result  by  decreasing  this  difference  to  slightly  less  that  Sms  or  4ms  to  hit  the  Merlin's 
4ms  update  window. 


6.0  JR3  Universal  Force/Moment  Sensor  System 


A  six  axis  force/moment  sensor  system  mounted  on  the  Merlin's 
tool  faceplate,  provides  3  force  components  (F„,Fy,F2)  and  3  moment  components 
(MxjMyjM^)  from  the  Merlin's  ena  effector  iiueraction  with  it's  environment. 
Adaitionally,  the  sensor  is  used  as  a  safety  limit  device  to  avoid  damage  to  the  Merlin's 
gripper  and/or  a  testbed  fixture.  Force/Moment  data  is  transmitted  to  the  host  system 
controller  via  a  9600  bits/ sec  RS-232  serial  link. 


7.0  Suggestions  for  Future  Work 

Currently,  course  positioning  experiments  using  the  MBA  exoskeleton  to  control  the 
Merlin  rooot  are  somewhat  limited  by  the  response  of  the  Merlin  to  rapid  changes  in 
position.  With  the  Merlin  end  effector's  position  and  orientation  controlled  by 
individual  joint  axis  controllers  servoing  only  to  achieve  a  commanded  motor  encoder 
count  (motor  shaft  position)  using  a  fixed  velocity  profile,  Merlin's  end  effector  response 
is  degraded  by  velocity  lag  or  positional  overshoot. 

The  HSHI  option  provides  a  Motor  Velocity  Command  that  could  be  utilized  to  improve 
Merlin's  response.  This  would  require  the  nost  system  controller  (Compaq  386)  to  close 
the  control  loop  on  the  robot  side  by  commanding  the  Merlin's  individual  joint  axis 
controllers  to  achieve  wrist  position/velocity  and  end  effector  orientation/angular 
velocity.  Merlin  motor  shaft  position  would  be  used  in  the  servo  loop. 
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9.0  Appendix  A:  MBAssociates  Backpack 


9.1  Electronic  Schematics 


This  page  intentionally  left  blank. 
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_ SYSTEMS  RESEARCH  LABS. _ 

TITLE 

MBA  EXOSKELETQN  BACKPACK  ELECTRONICS 
SIZE  I  DOCUMENT  NUMBER 

n  I  ROBOTICS  TELEPRESENCE 


DATE: 


DEC.  8.  1989 


SHEET 


1  OF  11 


T  RADIAL 


UPPER  ARM  ROLL 


SHOULDER  AZIMUTH  SHOULDER  ELEVATION 


■i-Ht  «i  w  if 


T  RADIAL 


UPPER  ARM  ROLL 


ELBOW 


SHOULDER  AZIMUTH 


SHOULDER  ELEVATION 


BOARD  TO  BOARD  CONNECTIONS 


1  SIGNAL  NAME 

DO 

^  01 

D2 
D3 

>  04 

I  05 

3  06 

5  07 

7  N/C 

3  N/C 

4.  6.  .  *20  ~  SIGNAL  GND 


J3  SIGNAL  NAME 

1  A1 

3  A2 

5  A3 

7  A4 

9  N.C. 

11  N.C. 

13  N.C. 

15  N.C. 

17  N.C. 

19  N.C. 

2.  4.  6.  .  .20  -  SIGNAL  GND 


\  SIGNAL  NAME 

12MHZ 

R/W\ 

0$ 

N.C. 

fl/W 

I  PH4\ 

3  RST\ 

5  ENCCNT\ 

"  ENCSTATX 

a  GRIPLEDX 

4.  6.  .  .20  -  SIGNAL  GND 


SIGNAL  NAME 


SYSTEMS  RESEARCH  LABS 


TITLE 

MBA  EXOSKELETON  BACKPACK  ELECTRONICS 


SIZE  DOCUMENT  NUMBER  Bi 

D  ROBTICS  TELEPRESENCE 


FILE  "  MBAS  .  04  "  I  DATE:  march  17.1989  SHEET 


3  OF  11 


INTERRUPT  HANDLER 


SN6109 


68HC000  MICROPROCESSOR 


SYSTEMS  RESEARCH  LABS 

TITLE 

MBA  EXOSKELETON  BACKPACK  ELECTRONICS 
(68HC000)  (SERIAL  I/O)  (BERH) 

(BATT  BACK-UP)  (HALT/RESET)  .  . 

SIZE 

DOCUMENT  NUMBER 

REV 

D 

ROBOTICS  TELEPRESENCE 

FILE  “MBA6.04"  DATE:  march  17.  iqbb 


7  OF  .11 


PART  NUMBER  QUANTITY 


MOTOROLA 

68HC000P12 

1 

SIGNETICS 

XR68C681CP 

1 

HEWLETT  PACKARD 

HCTL-200.0 

16 

FAIRCHILD 

74AC00 

1 

FAIRCHILD 

74AC02 

2 

FAIRCHILD 

74AC04 

4 

FAIRCHILD 

74AC08 

7 

FAIRCHILD 

74AC10 

1 

FAIRCHILD 

74AC11 

4 

FAIRCHILD 

74AC14 

1 

FAIRCHILD 

74AC20 

3 

FAIRCHILD 

74AC32 

5 

FAIRCHILD 

74AC74 

22 

FAIRCHILD 

74AC138 

2 

MOTOROLA 

74HC147 

1 

MOTOROLA 

74HC154 

1 

FAIRCHILD 

74AC191 

2 

FAIRCHILD 

74AC244 

3 

FAIRCHILD 

74AC245 

2 

FAIRCHILD 

74AC374 

4 

MOTOROLA 

74HC4075 

6 

MOTOROLA 

MC3486 

13 

MOTOROLA 

MC3487 

1 

LINEAR  TECHNOLOGY 

LT1081CN 

1 

INTERSIL 

ICM7555IPA 

1 

IDT 

IDT74FCT521 

4 

SMOS  SYSTEMS 

SRM20256LC-10 

4 

HITACHI 

27C64G-15 

2 

CATALYST  RESEARCH 

BIOOO 

1 

CATALYST  RESEARCH 

DRM-2 

1 

MOTOROLA 

2N6109 

1 

MOTOROLA 

2N2222 

4 

POWER  ONE 

SPL40-1005 

1 

BOURNS 

4116R-001-103 

12 

BOURNS 

1-104G 

4 

BOURNS 

4116R-001-471 

2 

ALLEN  BRADLEY 

RCR07G4750JS 

1 

ALLEN  BRADLEY 

RCR07G361JS 

1 

ALLEN  BRADLEY 

RCR07G105JS 

3 

ALLEN  BRADLEY 

RCR07G104JS 

3 

ALLEN  BRADLEY 

RCR07G6aiJS 

1 

ALLEN  BRADLEY 

RCR07G103JS 

4 

KEMET 

T350E106K025AS 

2 

KEMET 

C320C104K5R5CA 

5 

KEMET 

C330C103K1G5CA 

1 

SPRAGUE 

30GAQ15 

1 

KEMET 

T350A105K025AS 

4 

SPRAGUE 

1990224X9035AA2 

2 

B-D  CRYSTAL 

BD036BGB 

1 

SARONIX 

NCC060C-12 

1 

M-TRON 

MCO-T1-S3-1.0 

1 

CGK 

6125 

1 

6RAYHILL 

8744 

2 

CSK 

T101J12Q 

1 

CGK 

TioiJiaa 

1 

ARROW  HART 

1600R1E 

1 

C&K 

7101J12Q 

2 

CGK 

T101SH2Q 

2 

CGK 

T108SH2Q 

2 

BUSSMAN 

HTB24I 

1 

DIALIGHT 

521-9501-002 

16 

SAMTEC 

CA-02-SJC-B 

1 

TGB  ANSLEY 

609-2004 

8 

MOLEX 

22-04-1081 

2 

MOLEX 

10-89-1062 

16 

TGB  ANSLEY 

609-2041CE 

8 

MOLEX 

10-89-1068 

3 

MOLEX 

03-06-1023 

4 

MOLEX 

03-06-2023 

4 

BEI  MOTION  SYSTEMS 

E113-1024-20 

12 

B£I  MOTION  SYSTEMS 

E113-120-20 

4 

AMLAN 

CDS25L 

1 

AMLAN 

CDS37L 

1 

MOLEX 

03-06-2092 

16 

MOLEX 

03-06-1092 

46 

MOLEX 

02-06-2132 

200 

MOLEX 

02-06-1132 

200 

MOLEX 

16-02-0097 

100 

TGB  ANSLEY 

609-1014 

2 

TGB  ANSLEY 

609-1041CE 

2 

16-/32-BIT  MICROPROCESSOR 
DUAL  ASYNCHRONOUS  RECEIVER/TRANSMIT 
QUADRATURE  DECODER/COUNTER  INTERFAC 
QUAD  2  INPUT  NANO  GATE  /  AVAILABLE 
quad  2  INPUT  NOR  GATE  /  AVAILABLE  G. 
HEX  INVERTER  /  AVAILABLE  GATES:  U62 
QUAD  2  INPUT  AND  GATE  /  AVAILABLE  G. 
TRIPLE  3  INPUT  NAND  GATE  /  AVAlLABLi 
TRIPLE  3  INPUT  AND  GATE  /  AVAILABLE 
HEX  SCHMIDT  TRIGGER 
DUAL  4  INPUT  NAND 

QUAD  2  INPUT  OR  GATE  /  AVAILABLE  GA' 
DUAL  D  FLIP-FLOP  /  AVAILABLE  FLIP-FI 
1  OF  8  DECODER/DEMULTIPLEXER 
DECIMAL-  TO  -  BCD  ENCODER 
1  OF  16  DECODER/DEMULTIPLEXER 
UP/DOWN  COUNTER 

3-STATE  OCTAL  BUFFER/LINE  DRIVER 
OCTAL  BIDIRECTIONAL  TRANSCEIVER 
OCTAL  D-TYPE  FLIP-FLOP 
TRIPLE  3  INPUT  OR  GATE  /  AVAILABLE  ( 
QUAD  RS422  LINE  RECEIVER  /  AVAILABLt 
QUAD  RS422  DRIVER  /  AVAILABLE  PORTS: 
RS232  DRIVER/RECEIVER 
TIMER 

8-BIT  IDENTITY  COMPARATOR 
256K  BIT  STATIC  RAM 
B-BIT  EPROM 

2.BV  LITHIUM  IODINE  BATTERY 
BATTERY  BACKUP  MODULE 
PNP  POWER  TRANSISTOR 
NPN  TRANSISTORS 

5  VOLT  D.C..  8  AMP  POWER  SUPPLY  (LOC 

lOK  OHM  IX.  RESISTOR  PACKS 

lOOK  SIP  RESISTOR  PACKS 

470  OHM  DIP  RESISTOR  PACKS  (LOCATED 

475  OHM  RESISTOR.  1/4W  1% 

360  OHM  RESISTOR.  1/4W  5% 

IM  OHM  RESISTOR  1/4W  IX 
lOOK  RESISTORS.  1/4W  5X 
680  OHM  RESISTOR.  1/4W  5% 
lOK  RESISTORS.  1/4W  5% 
lOuF  CAPACITORS.  25V  TANT . 

. luF  CAPACITORS.  25V  CERAMIC 
.OluF  CAPACITOR.  BOV  CERAMIC 
15pF  CAPACITOR.  IKV  CERAMIC 
luF  CAPACITOR.  25V  TANT. 

.22uF  CAPACITORS.  25V  TANT. 

3.6864  MHZ  CLOCK  CRYSTAL 
12.0  MHZ  OSCILLATOR 

1  MHZ  OSCILLATOR 
SPST  RESET  SWITCH 
DIP  SWITCHES 
DEBUG  SWITCH 
SRAMWRPRO  SWITCH 
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9.0  Appendix  A:  MBAssociates  Backpack 
9.2  Timing  Diagrams 
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mba.c68 


^  5|e  >je  >|e  j|<  jjc  :|c  ♦♦♦♦  3|c  ♦♦♦♦♦♦♦  3K  sic  ****♦♦*♦♦  ♦ 


FILE  mba.c68 


FUNCTION  main 
AUTHOR  Todd  Mosher 
DATE  5-6-91 


GENERAL  DESCRIPTION 

This  program  runs  in  the  mba  back  pack. 

This  routine  will  wait  for  the  user  to 
move  the  joints  through  their  indexs  and 
then  it  will  return  the  position  of  the  encoders 
with  respect  to  their  index  position. 

Die  )ic  sic  s|c  sic  s|c  sjc  sic  9|c  sK  sK  sfe  a|c  9(e  )|e  sic)|(  )|c){c  He  sic  sic  sK  sic  sK  s|c  sic  sic  s|c  sic  )K  s|c  sic){c)ic  9ic  9|<  sic  sk  9|e  9|c  sk  sk  s|c  9(e  sicsic  })c  ^  )|e  9|e  sic  sic  sioic^ 

jWnclude  <stdio.h68> 

^include  <c^^.h68> 

^define  ESTOP  0x0024 
^define  GOHALT  0x0009 


^define  chr  inQ  ((iostatM)  &  0x01)  /*  check  char  avml  status  */ 

^define  get~byt0  (ioport[0])  /*  look  at  the  incoming  char  */ 
int  *bptenc;  /*  pointer  to  optical  encoder  values  */ 

int  *^tsta;  /*  pointer  to  optical  encoder  status  */ 

char  ’•loport; 
char  *iostat; 
int  *swt; 
int  *lights; 

int  glob_pos[16];  /*  position  from  index  mark  */ 

int  Iast_pos[l6];  /*  last  encoder  val  read  */ 

int  reqpnd; 

int  sendQ; 

intinit  oeQ; 

int  posTtionO; 


mainQ 

int  coment  =  0; 

int  tog  =  0; 
int  off  =  Oxffff; 

int  on  =  Oxfefe;  /*  only  gripper  light  on  */ 
int  lit_mask; 


for  (i=0;i<  16;i-l--l-) 
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mba.c68 


glob_pos[i]  =  0; 

Iast_pos[i]  =  0; 

optsta  =  0x24020; 
ioport  =  0x240e7; 
iostat  =  0x240e3; 

5^1;  =  0x24080* 

lights  =  0x24040;  /*  point  to  lights  */ 

optenc  =  0x24000;  /*  point  to  optical  encoders  *! 

reqpnd  =  0; 

init_oeQ;  /*  initialize  optical  encoders  */ 

lit  mask  =  on; 

♦lights  =  lit_mask; 

while  (1)  /♦  do  forever  */ 

{ 

if  (chr  inO  1 1  reqpnd)  /*  if  char  in  the  serial  io  port  ♦/ 

if  (comcnt+  4-  =  =  100)  /*  show  comm  with  blinking  lights  */  lit_mask  =  lit_mask  & 

Oxefef; 

if  (comcnt  =  =  200) 

litmask  =  on; 
comcnt  =  0; 

}  . 

tog  =  tog; 

if  (tog) 

lit_mask  =  lit_mask  &  0x7f7f; 
else 

lit  mask  =  lit  mask  |  0x8080; 

♦lights  =  litmask; 

if  (Ireqpnd)  /♦  if  a  request  was  pending  ♦/ 

i  =  get_byt0; 
else 

i  =  reqpnd; 

if  (i  ==  'H'  1 1  i  ==  'B'  1 1  i  ==  ’L’  1 1 

i  =  =  'R'  II  reqpnd)  /♦  pc  requesting  data  ♦/ 

reqpnd  =  0; 

if  ((swt[0]  &  GOHALT) !  =  GOHALT)  /♦  z  for  sleep/halt  ♦/  send("Z" ,  1); 
else  if  ((swt[0]  &  0x0012)  =  =  0x0012)  /♦  no  index  swt  pressed  */  sendC'H",!); 
else  if  ((swt[0]  &  0x0012)  =  =  0)  /♦  both  indexs  pressed  ♦/ 
send("R" ,  1) ;  I*  reset  indexes  ♦/ 

else 

send("I",l);  /♦  indexing  mode  ♦/ 

if(i  ==  'H'  1 1  reqpnd  ==  'H') 
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for  (i=0;i  <  16;i+  +)  /*  send  out  all  data  */ 

send(&glob_pos[i],2);  /*  return  an  error  code  *! 
positionO;  /*  get  new  encoder  positions  */ 

else  if  (i  =  =  'L'  1 1  reqpnd  =  =  'L') 
for  (i =0;i  <  8;i  +  +)  /*  send  out  all  data  */ 

send(&glob_pos[i],2);  /*  return  an  error  code  */ 
posibonO;  /*  get  new  encoder  positions  ♦/ 

else  if  (i  =  =  'R'  1 1  reqpnd  =  =  'R') 
for  (i=8;i<  16;i-l-+)  /*  send  out  all  data  */ 

send(&glob_pos[i],2);  /*  return  an  error  code  *! 
posiUonQ;  /*  get  new  encoder  positions  *! 


else  if  (i  =  =  ’S’)  /*  pc  requesting  status  */ 

send("S",l); 


positionO;  /*  get  new  encoder  positions  */ 

/*  if  |(swt[0]  &  ESTOP) !  =  ESTOP)  /*  allow  user  to  get  out  on  */ 

I*  exit(0);  /*  emergency  stop  */ 


/* 


} 


while  ((swt[0]  &  GOHALT) !  =  GOHALT)  t*  send  nothing  for  gohalt  */ 
positionO;  /♦  keep  track  of  position  ♦/ 

1  =  1; 

if  01) 

pnntf("  ");  /*  this  is  because  of  a  bug  in  the  libs  */ 


int  positionO 

int  cur_pos; 
inti; 


for  (i=0;i  <  16;i+  +)  /*  for  all  encoders,  calc  their  positions  */ 

cur_pos  =  optencp]; 

if  (cur  pos  >  =  last  pos[il)  /*  clock  wise  or  else  ccw  past  index  */ 
if^lastposp]  -F  2046)  >  curpos)  /*  cw  motion  */ 
glopjx)s[i]  +  =  cur_pos  -  lastjiosp]; 
else 

glob_pos[i]  -=  last_pos[i]  +  4096  -  cur_pos;/*  ccw  past  index*/ 
else  /*  ccw  or  else  clock  wise  past  index  */ 

if  ((cur  pos  +  2046)  >  =  last_pos[i])  /*  ccw  motion  */ 
gl6b_pos[i]  -=  last_pos[i]  -  curjpos; 
else 
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} 


glob_pos[i]  +  =  cur_pos  +  4096  -  lastjK)s[i];  /*  cw  through  index  */  last_pos[i] 


intinit  oeO 

{  " 

int  list[16]; 

int  tmp; 
int  i; 
intj; 


/*  initialize  the  optical  encoders  */ 

/*  set  bits  to  1  which  will  cause  the  */ 

/♦  hw  to  reset  counters  when  the  index  is  *! 
I*  crossed  */ 


for  (i=0;i<  16;i++) 
list[i]  =  0; 

♦lights  =  0;  /*  turn  all  lights  on  */ 

♦optsta  =  -1;  /*  set  all  bits  to  1  to  allow  the  hardware  */ 

tmp  =  optsta[0];  /*  to  reset  the  counters  when  the  index  is  passed  */ 
/*  while(optsta[0])  /*  while  not  all  indexs  crossed  */ 
while(optsta[0]  &  OxffOO)  /*  while  not  all  indexs  crossed  */ 


{ 


♦lights  =  ■(♦optsta);  /♦  lights  refle 
if  aswt[0]  8l  0x0012)  !  =  0x0012) 


reflect  the  status  to  users  ♦/ 

/♦  index  switch  aborts  ♦/ 

/♦  this  is  incase  of  broken  encoder  ♦/ 
optsta[0]  =  0; 

for  (i=0;i<  16000;i+  +);  /♦  delay  for  swt  release  ♦/ 

j  =  1; 

for  (i=0;i<  16;i+  +)  /♦  which  ones  were  crossed  */ 

if  ((♦optsta  &  j  ==  0)  &«&  !list[i])  /♦  new  index  crossed  ♦/ 

list[i]  + +;  /♦  note  that  index  crossed  */ 

glob_pos[i]  =  optenc[i];  /♦  set  global  position  info  ♦/  last_pos[i] 

J  j=j<<i; 

positionO; 

if  (chr  inO)  /♦  if  there  is  a  char  in  the  serial  io  port  ♦/ 

i  =  =  'L'  1 1  i  =  =  'R')  /♦  pc  requesting  data  ♦/ 

reqpnd  =  i;  /*  note  the  pending  request  ♦/ 

else  if  (i  =  =  'S')  /♦  pc  requesting  status  ♦/ 

^  send("S",l); 


cur_pos; 


globj30s[i]; 
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mt  send(byt,cnt)  /*  send  cnt  bytes  out  the  serial  io  port  */ 
char  *byt;  /*  if  cnt  is  0  then  an  ascii  string  is  being  */ 

int  cnt;  I*  sent  */ 

{  .  . 

mt  i; 

longj; 

if  (lent)  /*  sending  a  text  string  */ 

cnt  =  strlen(byt); 

for  (i=0;i<cnt;i++)  /*  send  out  all  bytes  */ 

^  for  0  =0;j  <  640001;] + +)  /*  wait  for  tx  free  */ 

if  ((iostat[0]  &  6x04)  =  =  0x04)  /*  tx  ready  */ 

if  (j  <  64b001)  /*  max  wait  for  tx  ready  */ 

ioport[0]  =  byt[i]; 
else 

retum(l);  /*  return  error  */ 
retum(0); 
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c68  +fi  -o  mba.r68  mba.c68 

ln68  +c  1000  -t  -o  mba.e68  mba.r68  -lc68s 
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set  INCL68= c:\aztec_c\include 
set  CLIB68=c:\aztec_c 
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"C" 

"68000" 

^EXTENSIONS  ON$ 
j5f$FULL  LIST  ON$ 
#$ENTRY  OFF$ 
#$LIST  CODE  ON$ 


*1 


/* 

*/ 

/*  DESIGNED  BY  :  TODD  MOSHER  /  MONTY  CRABILL 

1* 

*/ 

/*  CODED  BY  :  TODD  MOSHER 

*1 

1* 

*1 

/*  DATE  :  JAN-16-1991 

*1 

/* 

*1 

/*  REVISION  NUMBER:  ORIGINAL 

*/ 

1* 

*/ 

I* 

/* 

I* 

/* 

/* 


REVISIONS  LOG; 
NUMBER  DATE 


DESCRIPTION 

♦  / 

RL  C  MBA 


*/ 


*/ 


/* 

I* 

I* 

/* 

/* 

/* 

/* 

I* 

I* 

/* 

/* 

/* 

I* 

/* 

I* 

/* 

I* 

I* 

I* 

I* 

/* 

/* 

/* 

I* 

I* 


*  / 

THIS  PROGRAM  IS  FOR  THE  MBA  BACKPACK.  IT  IS  A  MONITOR  */ 
WHICH  WILL  INITIALIZE  THE  ENCODERS  AND  THEN  WAIT  FOR  A  */ 
FILE  TO  BE  UPLOADED  OVER  THE  SERIAL  PORT.  DURING  */ 
INITIALIZATION  OF  THE  ENCODERS ,  ALL  LEDS  WILL  BE  LIT.  */ 

AT  THIS  POINT,  ALL  JOINTS  MUST  BE  ROTATED  THROUGH  THEIR  */ 
RANGE  OF  MOTION  SO  THAT  THE  LEDS  WILL  GO  OFF.  */ 

THEN  THE  LEDS  WILL  CONTINUE  TO  FLASH  UNTIL  A  FILE  STARTS  */ 
UPLOADING.  WHILE  THE  FILE  IS  LOADING,  THE  LIGHTS  WILL  DO  */ 

A  COUNTING  PATTERN,  AND  FINALLY  THEY  WILL  BE  TURNED  OFF  */ 
WHEN  THE  UPLOADED  PROGRAM  BEGINS  EXECUTION.  */ 

ERROR  CODES  WILL  BE  DISPLAYED  ON  THE  LEFT  HAND  */ 

ERROR  CODES:  */ 

LIGHT  ERROR  */ 

GP  "PGM"  PREFACE  TO  FILE  TX  MISSING  */ 

WF  STARTING  ADDR  OF  CODE  MISSING  */ 

WR  CODE  SIZE  MISSING  */ 

LR  STARTING  ADDR  OF  DATA  MISSING  */ 

EB  DATA  SIZE  MISSING  */ 

UA  NOT  ENOUGH  CODE  SENT  UP  */ 

SE  NOT  ENOUGH  DATA  SENT  UP  */ 

SA  CHECK  SUM  ERROR  */ 

^  / 

SEE  UPLOAD  FUNCTION  HEADER  FOR  EXPECTED  INFO  FOR  FILE  TX  */ 

*1 


y  :(e  :|e  5j<  **  ^  sic  5|c  :1c  }(c  5|c  5ic  :ic  sic  *  ****  ****************  **  :|<  sic  :(c  ***********  / 
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#define  DUART  A  0x0240E7 
#define  DUAR'TB  0x0240F7 
#define  TXR  MASK  0x04 
#define  SND~MASK  0x01 

/♦char  *jstaclc  =  0x20000:  /*  FOR  EPROM  0-3  MUST  BE  STACK  POINTER  *!  int  mainO; 
/♦char  *jaddr  =  main;  /*  FOR  EPROM...  4-7  MUST  BE  PGM  PC  ADDR  */ 

/*  MUST  BE  20100  IN  EPROM  BITS  4-7  */ 

/♦char  ♦jaddr  =  0x30228;  FOR  EMULATION  ♦/ 
extern  int  STRTQ; 

#$ORG  0x24080$ 

int  switches;  /*  gripper  switches  bit  0  -  left  halt/go  */ 

/*  bit  1  -  left  indx  */ 

/*  bit  2  -  left  E  stop  */ 

/*  bit  3  -  right  halt/go*/ 

/*  bit  4  -  right  indx  */ 

/*  bit  5  -  right  E  stop  */ 

/*  note  debug  is  bit  0  @  0x24060  */ 

#$END  ORG$ 

#$ORGDx240E3$ 
short txr  statptr; 

#$END  DRG$ 

#$ORG“24020H$ 
int  oe_sta; 

#$END  ORGS 
short  *dev  ptr; 
char  tarr[4T; 

char  (*usrpgm)0;  /*  pointer  to  user  program  ♦/ 

int  glberr:  /*  general  pu^se  error  indicator  ♦/ 

int  *grp_loa;  /*  addr  of  gnper  leds  */ 

int  gipdsp; 
char  ♦cadr; 

char  sndstr[100];  /*  string  to  send  from  function  send  */ 
int  waitO;  /*  waits  for  tx  ready  */ 

int  sendO; 

int  getbytO;  /*  byte  from  serial  port,  -1  =  time  out  */ 

/*  int  init  oeQ;  /*  optical  encoder  initialization  ♦/ 

int  uploadO;  /*  attempts  to  upload  and  execute  the  usr  pgm  */ 

mainO 

{ 

short  z; 
short  j; 
int  k; 

short  ♦oe_ptr,i; 
short  *c  r  ts; 
grp  led  =  ^nt  ♦)0x24040; 
gipdsp  =  0: 

c  r  ts  =  (snort  *)0x240El;  /*  setup  pointers  ♦/ 

dev_ptr  =  (short  *)DUART_A; 
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STRTO;  /*  CALL  STRT  TO  INITIALIZE  SERIAL  PORT  */ 

glberr  =  0; 

*c_r  ts  =  0x13; 

*c_r_ts  =  Ox  IF; 

k  =  0; 

while(l)  /*  wait  for  a  program  to  be  uploaded  */ 

^  if  ((switches  &  0x0012)  !  =  0x0012)  I*  index  clears  the  err  flag  */  glberr  =  0 


} 


/*  if  no  error,  flash  leds  */ 


&  SND  MASK);  /*  get  rs422  status  */ 
7*  char  rx  from  PC  */ 

/*  turn  lights  off  */ 

/*  upload  and  exec  user  program  */ 

/*  on  error,  send  error  msg  */ 


exomon.c 


yi|cs|eai<)|(3(C)k)|e9|c}(cs|e^9|c:|c3|cHc9ie9le)|C9|c){C9ie^^)iC3ie3ie3|c3{e9|c)|ejica(caie9|c)|(:ieHe9k)|<)ie^:|C9|«)|C3K^9|«9ie^3K3fc3ie9|e3ic9(e3(c9K9{e3ie^3{e:ie:|c9|c  j 


/* 

/* 

/♦ 

/* 

/* 

/* 

/* 

/♦ 

/♦ 

/* 

/* 

/* 


♦/ 

TfflS  ROUTINE  WILL  CONTROL  THE  UPLOADING  OF  A  PROGRAM 


THIS  ROUTINE  EXPECTS 
BYTES  0-2  PGM 
BYTES  3-6  CODE  START  ADDR 
BYTES  7-10  BYTES  OF  CODE 
BYTES  11-14  DATA  START  ADDR 
BYTES  15-18  BYTES  OF  DATA 
CODE  DATA 
DATA  DATA 
1  BYTE  CHECK  SUM 


♦/ 


*/ 

*/ 


*/ 


*/ 


*/ 


*/ 


*/ 


*! 


/*  THE  PC  UPLOAD  PROGRAM  ALWAYS  SENDS  OUT  AN  'S'  BEFORE 
/*  STARTING  THE  UPLOAD.  THIS  IS  TO  STOP  ANY  PROGRAMS  WHICH 
/*  MAY  ALREADY  BE  RUNNING  IN  THE  BACK  PACK  */ 

/♦  *! 


/♦  AFTER  EACH  THING  LISTED  ABOVE,  AN  'OK'  OR  'ER'  WILL  */ 

/*  BE  SENT  TO  THE  SENDING  DEVICE,  ON  'ER' ,  THIS  ROUTINE  */ 
/♦  WILL  RETURN  TO  MONITOR  MODE  */ 

/♦  ♦/ 

y  3ie  )|C  ♦  ♦  9|(  3l(  ♦  9k  He  ♦  )|e  ♦  3|e  )ic  )ic  )ic  9|c  3|e  He  3k  9ie  9(c  9)c  aK  3(c ))( )|e  }ic  :)c  9fe  9|( :)( Sle  :}c  9|c  :fe  He  xe  )i(  9ie  )ie  sic  9ic  9ie  Me  9ic/ 


int  uploadQ 
longi,j; 

char  chksum  =  0; 
/*  char  *cadr  =  01; 
long  csiz  =  0; 
char  *dadr  =  01; 
long  dsiz  =  0; 
long  tlong; 
long  tmp; 
int  err  =  0; 


/*  code  address  ♦/ 

/*  size  of  code  in  bytes  */ 
/*  data  address  */ 

/*  data  size  */ 


*/ 


*/ 

♦/ 


IfoM 

retum(l);  /*  ignore  initial  S's  */ 

if  (i  !  =  'P'  1 1  /*  must  start  with  PGM  */ 

|SS5S8l=:S'.i' 

retum(glberr  =  (*grp  led  =  Oxfffe));  /*  return  error  */ 
sendC'OK"); 
cadr  =  01; 
tlong  =  01; 

for  (i=3;i>  =0;i-)  /*  get  code  start  address  */ 

if  ((tarr[i]  =  getbytO)  =  =  -1)  /*  -1  =  no  data  available 

*!  retum(glBerr  =  (♦grpjed  =  Oxfffd)); 


else 


dong  =  dong  *  256  +  tarr[i];  /*  build  code  address  */ 


tlong  =  tlong  *  j> 
cadr  =  (char  *)tIong; 
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sendC'OK"); 


for  (i=3;i>  =0;i--)  /*  get  code  size  in  bytes  */ 

if  ((tmp  =  getoytO)  ==  -1)  /*  -1  =  no  data  available  */  retum(glberr  =  (*grp_led 

Oxfffb)); 
else 

csiz  =  csiz  I  (tmp  <  <  (8  *  i));/*  build  code  size  */ 
sendC'OK"); 


for  (i=3;i>  =0;i--)  /*  get  data  start  address  */ 

if  ((tmp  =  getoytO)  =  =  -1)  /*  -1  =  no  data  available  */  retum(glberr  =  (*grp_led 

0xfff7)); 
else 

tlong  =  tlong  I  (tmp  <  <  (8  *  i));  /*  build  data  address  */  dadr  =  (char  *)tlong; 
sendC’OK"); 


for  (i=3;i>  =0;i--)  /*  get  data  size  in  bytes  */ 

if  ((tmp  =  getoytO)  =  =-l)  /*-l=no  data  available  */  retum(glberr  =  (*grp_led 

Oxffef)); 

else 

dsiz  =  dsiz  |  (tmp  <  <  (8  *  i));  /*  build  data  size  */ 
sendC’OK"); 


for  (i=0;i < csiz;i+  +)  /*  load  in  all  code  */ 

if  ((tmp  =  getbytO)  =  =  -1) 

return  (glberr  =  (*grp_led  =  Oxffdf)); 
else  7*  no  error  so  continue  */ 

^  if(grpdsp++  >32760) 
grpdsp  =  0; 

*grp_led  =  grpdsp; 
enksum  +=  tmp; 
cadr[i]  =  tmp; 

send|"OK’'); 

for  (i=0;i<dsiz;i+  +)  /*  load  in  all  data  *! 

if  ((tmp  =  getbytO)  =  =  -1) 

return  (glberr  =  (*grp_Ied  =  Oxffbf)); 
else  7*  no  error  so  continue  */ 

^  if  (grpdsp ++  >  32760) 
grpefsp  =  0; 

*grp_led  =  grpdsp; 
enksum  +  =  tmp; 
dadr[i]  =  tmp; 

send|"OK"); 
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if  (chksum  =  =  getbytQ)  /*  verify  check  sums  of  code&data  *! 

^  sendC'OK"); 

*grp  led  =  Oxffff;  /*  turn  leds  off  */ 

/*  while  ((switches  &  0x0012)  =  =  OxOiOll);  DEBUG  ONLY  */ 
usrpgm  =  cadr;  /*  set  up  jump  to  user  program  */ 

usrpgmO;  /*  go  execute  user  program  */ 

else 

retum(glberr  =  (*grp  led  =  OxffTf)); 
retum(l);  7*  all  done,  go  back  to  monitor  */ 


*  / 

THIS  ROUTINE  WILL  SEND  OUT  THE  REQUESTED  DATA  FROM  THE 
BUFFER.  ^ATER  ON  THIS  SHOULD  BE  CONVERTED  INTO  AN  */ 
INTERRUPT  DRIVEN  ROUTINE)  *! 

*1 


I* 

I* 

I* 

/* 

I* 

int  send(str) 
char  *str; 

{  .  . 

int  i; 


*/ 


for  (i=0;str[i]  !  =  0;i+  +)  /*  for  all  data  points  */ 

waitO;  /*  wait  for  tx  ok  status  */ 

*dev3tr  =  str[i];  /*  send  the  data  byte  */ 


3K  5|<  jK***  ********************************’*<  ***************/ 


/*  */ 

/*  THIS  ROUTINE  WILL  WAIT  FOR  AN  INCOMMING  CHARACTOR  FROM  */ 
/*  THE  SERIAL  PORT.  THIS  SENDS  BACK  THE  CHAR  BEING  RX  AND  */ 

/*  WILL  SEND  BACK  A  -1  ON  TIME  OUT.  THE  TIMEOUT  CHOSEN  IS  */ 

/*  SOME  WHAT  A  RANDOM  CHOICE  WHICH  HAS  BEEN  PROPERLY  ADJUSTED 
/*  DURING  TESTING.  */ 


/* 


*/ 


int  getbytO 

int  i,j,ch_in; 
int  status; 

for  (i=0;i<  12000;i++) 

^  for  (j=0;j<50;j  +  +) 

if  ((txr  stat_ptr  &  SND_MASK)  =  =  1)  /*  char  rx  */ 
break^; 


*/ 
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if  (j  <  50)  /*  yes  char  rx  ♦/ 

break; 


if  (i  <  12000)  /*  yes  char  rx  */ 

retum(*dev_ptr  &  OxOOff);  /*  return  char  */ 

retum(-l);  /*  else  no  char  rx  */ 


/* 

/* 

/* 

/* 


*/ 


THE  ROUTINE  WAIT  POLLS  THE  XMIT  BUFFER  STATUS  WAITING  FOR 
A  FREE  TRANSMIT  BUFFER.  */ 

*/ 

int  waitO 

{ 

^  while(txr_stat_ptr  &  TXR_MASK  !  =  4); 


*! 


/*  */ 

/*  THE  ROUTINE  INIT  OE  MAPS  THE  OPTICAL  ENCODERS  IN  THE  */ 

/*  MEMORY.  THE  ROUTINE  THEN  WAITS  FOR  EACH  OF  THE  OPTICAL 
I*  ENCODERS  TO  BE  MOVED  THROUGH  THEIR  ZERO  POINTS.  UPON 
/*  COMPLETION  OF  THIS  INITIALIZATION  PROCEDURE  CONTROLL  IS 
/*  RETURNED  TO  THE  CALLING  ROUTINE.  */ 


/*  - not  used  any  more  -  this  taken  care  of  by  uploaded  */ 

/*  program  -  */ 

y  5|c  Jlc  ^  ♦  }I<  3|c  ♦#♦*  ^*  *  *♦♦♦♦♦♦***  ♦♦♦♦♦♦♦  sR*  ♦♦*♦♦***♦♦*♦*♦♦  9l«  ★  sk  ♦  He*****  He  ^ 


int  init_oe() 


‘$ORG  24000H$ 


int 

Igp  cnt; 

int 

IwF  cnt; 

int 

Iwr  cnt; 

int 

Ur  cnt; 

int 

leb  cnt; 

int 

lua  cnt; 

int 

Ise  cnt; 

int 

Isa  cnt; 

int 

rgp  cnt; 

int 

rwFcnt; 

int 

rwr  cnt; 

int 

rlr  cnt; 

int 

reb  cnt; 

int 

rua  cnt; 

int 

rse  cnt; 

int 

rsa  cnt; 

*! 

*1 

*/ 
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*grp_led  =  0; 
oe_sta  =  -1; 
do 


0x0012)  I*  user  may  abort  -  indx  sw  */  break; 


10.0  Appendix  B:  MBA/Merlin  Control  Software  Listing 
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merlin 

testsys 

getjr3 

prockey 

utime 

keycont 

movearm 

calibmer 

mercmds 

window 

forkin 

sinvkin 

merinit 

merltmat 

utils 

enctorad 

fullsys 

transp 

wristang 

merlrmat 

jointang 

mbainit 

rexotmat 

calib 

getmba 

ttyopen 

ttylisr.obj 

tmwinl.lib 

1  win,  lib 
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^S|C5!<5|< >!<*♦*♦♦♦******♦***♦*************  ********** 


FILENAME:  Merlin.C 

PROGRAM:  (main) 

AUTHOR:  TW  Mosher  MLCrabill 


DATE:  4  April  91 

DESCRIPTION: 


:|cs|«**5jc:lc5|c****Hc****************************************/ 


#include  <stdio.h> 
#include  <conio.h> 
#include  <math.h> 
^include " merlin. def 


extern  int  forkin();  /*  forward  kinematic  function  */ 
int  cbreakO  /*  control  the  ctrlc  vector  ♦/ 

{ 

wn_exit(); 

tty_close(}; 

exit(printf("User  aborting  program\n")); 

mainO 

^  long  tst[6]  =  {0}; 
intj,i,k,l; 


window  initO;  /*  bit  3  window  init  */ 
wn  initO;  /*  graphics  screen  init  */ 
merinitO;  /*  vanable  initialization  */ 
clrscrO; 

ctrlbrk(cbreak);  /*  setup  Ctrl  c  vector  */ 
mer_init_serv();  /*  init  servo  values  -  gain  ...  */ 


while(l) 

/*  fill  in  main  menu  */ 
fill  menu(25,5,"  HSHI ", 

"  "CALIBRATE 

"DIAG  FORWD  KIN  ", 
"INVERSE  KIN 
"MOVE  TO  ZERO  POS  ", 
"FULL  SYSTEM 
"TEST  SYSTEM 
"ZZZ  -  do  not  use  ", 
"EXIT  &  ZERO  POS  ", 
"TERM  HSHI  &  EXIT  ", 
"QUIT 
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NULL);  /*  must  end  with  null  *! 

switch(menu())  /*  display  menu  and  get  option  selected  */ 

case  0:  /*  calibrate  *! 

calib  merO; 
breaF; 

case  1:  /*  do  fdw  din  */ 

mer  init  servQ;  /*  set  0  deg  here  */ 
i  ='statul(l,ll,"  FORWAI^  KINEMATICS  INFO  ",75,12); 
key_controlfforkin,&j); 
close_status(); 
break; 

case  2:  /*  do  merlin  inverse  kinematics  */ 

mer  init  servO;  /*  set  0  deg  here  */ 
j  ='statul(l,8,"  INVERSE  KINEMATICS  INFO  ”,75,12); 
while(I) 

^  fill  form(l, I, "X  POSITION  ",-l,-l,’D’,6,&xj)Os); 

fiirform(l,3,"Y  POSITION  ",-l,-l,'D’,6,&yjx)s); 

fiirform(l, 5, "Z  POSITION  ",-l,-l,'D’,6,«fez_pos); 

form  disp("  TOOL  TIP  POSITION  ","",1,1, 

25,3,25,8);  /*  display  initial  form  */ 
form_exe(&i,0);  /*  get  user  input  for  xyz  *! 
form  closeO; 

/*  in^n(j,x_pos,yjx)s,z_pos);  */ 

more  points  (y)-.NULL) 

if  (i  !=  'Y') 

break; 

closestatusO; 

break; 

case  3:  I*  move  joints  to  zero  position  */ 

mer  closeQ;  /*  sets  joints  to  0  position  */ 

bredk; 

case  4:  /*  full  exo  master  to  merlin  slave  system  */ 

mer  init  servQ;  /*  set  0  deg  here  */ 
full_system();  /*  full  up  control  */ 
mer  closeQ; 
break; 

case  5:  /*  full  test  system  */ 

mer_init_serv();  /*  set  0  deg  here  */ 
test_system();  /*  full  up  control  */ 
mer  closeQ; 
break; 

case  6:  /*  test  code  *! 

mer  init_serv();  /*  set  0  deg  here  */ 
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} 


while  (1) 

{ 

fill  form(l,l, "option 
fili;form(l  3,-deg  , 
form_disp(  max  joint  tst ,  1 , 1 . 

25,;3>25,8);  /*  display  initial  form  */ 
form  exe(&i,0);  /*  get  user  input  for  xyz  */ 
form"close(); 
if(k  >  1) 

.  break; 

if  (k  =  =  0)  /*  option  move  to  deg  */ 

for  (i=0;i<3;i++) 
tst[i]  =  j  *  276; 
movearm(tst, 'E'); 

else  /*  move  a  deg  each  time  */ 

{ 

for  (i=0;i<3;i++) 
tstri]  =  0; 
for  0=0;i<j;i++) 
for  (1=0;1<3;1+ +) 

^  tstD]+=  276; 
movearm(tst, 'E'); 

}  ’ 

wind(2,2,"pause",'E','  ",NULL); 
for  (i=0;i<3;i++) 
tst[i]  =  0; 
movearm(tst,'E'); 


L 


} 


yreak; 
case  7: 
mer_close(); 
wn  exitO; 
exit(0); 
case  8: 
mer_close(); 
mer_hshi_exitO; 

wn  exitO; 
exir(0); 
default: 
wn_exit(); 
exit(0); 
break; 


/*  sets  joints  to  0  position  */ 
/*  close  window  stuff  */ 


/*  sets  joints  to  0  position  */ 
/*  terminates  the  hshi  pgm  */ 
/*  close  window  stuff  */ 
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l**)is**H:****************************************************** 


FILENAME:  fullsys.c 

FUNCTION  NAME:  full_system 
AUTHOR:  Todd  Mosher  &  Monty  Crabill 

DATE;  April  23  1991 

DESCRIPTION:  .  ^ 

This  program  will  allow  a  user  to  control  the  merlin  robot 
using  the  mba  exo-skeleton, 

^include  <stdio.h> 

^include  <conio.h> 

^include  <math.h> 


#include  "merlin, ref" 

extern  int  stop_print;  /*  this  is  a  var  in  the  tmwin,lib  */ 

/*  used  to  stop  screen  prints  */  unsigned  int  utime(); 
extern  float  data[3][2000]; 
extern  int  datcnt; 
extern  int  trigr; 

float  flimit(val) 
float  val; 

if  (val  >  1) 
retum(l); 
if  (val  <  -1) 
return  (-1); 
retum(val); 


int  test_system() 

long  tm; 
char  str[100]; 
unsigned  int  prime, ctime; 
int  i,i,k,wptr; 
double  X  =  15; 

double  y  =  18;  /*  y  =  0  causes  sqrt  error  —  fix  for  normal  operation  */ 

double  z  =  7; 
double  usedx  =  0; 
double  zforce; 
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double  lastx,lasty,lastz; 
long  waste[6]; 
double  opinp[4]; 
double  resmat[4]; 

FILE  *fp[3]; 
unsigned  int  cnts; 
double  Ferror  =  0; 
float  Fdesired  =  0; 
double  Fscale  =  0; 
double  xd  =  0; 
double  prevxj)revy,prevz; 
float  vel  =  85: 
float  Ftol  =  .01; 
float  gain  =  27; 


/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

I* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/* 

/♦ 

/* 

/* 

*/ 


mba  rqst  =  'L'; 

j  =^’  « 

datcnt  =  0; 
trigr  =  0; 
clrscrQ  * 

printfC’W  Loading  Exo  Program\n"); 
s^ystemC’upload  mba.e68^);  /*  load  exo  pgm  */ 

if  ((fb[0]  =  fopen(''upload.sta","r"))  ==  NULL) 

^  printfC'Disk  error  or  upload  version  error\n"); 
printf("Press  return  to  continue,  Ctrl  c  to  abort\n"); 
getcharO; 

lscanf(fp[0] , "  %d"  ,«fei);  /*  get  result  of  upload  to  mba  */ 

fclose(fp[0]); 

if  (i  >  ())  /*  upload  problem  */ 

retum(wind(8,8,"COMM  ERR",'E',’  ',"Can  not  communicate  with  MBA", NULL)); 

tty_open(2, 10,8,1,0) ;  /*  setup  the  serial  port  for  mba  */ 

tty_out(2,mba_rqst);  /*  start  handshaking  with  EXO  */ 

if  (i  =  =  0)  /*  Exo  program  newely  loaded  */ 

^  wind(8,8,"INITIALIZE'VE','  ', "Move  all  joints  through  their", 

"Full  range  of  motion", NULL); 

^  calib_mba();  /*  Calibrate  system  */ 

clrscrQ; 

fill  menu(25, 8,  "WRIST",  /*  setup  the  menu*/  .  ^ 

"LOCK  WRIST  ON  ",  /*  turn  pnnts  on/off  for  speed  */ 
"LOCK  WRIST  OFF  ", 

NULL); 
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lock  wrist  =  !menu(); 

Till  form(l,  1, "Desired  force  ",-l,-l,'F',6,&Fdesired); 
fiirform(l, 3, "Desired  Vel  in/sec  ","  ",-l,-l,’F',6,&vel); 
fill  form(l, 5, "Force  tolerance  ",-l,-l,'F',6,&Ftol); 
fiirform(l  7,"Gain  (1-100)","  ",-l,-l,'F’,6,&gain); 
form  disp(^'  Force  control  params  ",1,1, 

25.3,30,12);  /*  di^lay  initial  form  */ 
form_exe(&i,0);  /*  get  user  input  for  xyz  */ 
form  closeQ; 

xd  =  vel  /  250.0;  /*  4  ms  loop  time  =  250  */ 

if  (gain  =  =  0) 

gain  =  1; 

Fscale  =  100.0  /  gain; 


/*  stopprint  =  1;  */ 

/*  while  (!kbhit())  /*  debug  only  *! 

^ 

/*  outportb(0x300  -f-  4,0x00); 

/*  out^rtb(0x300  -I-  4,0x02); 

/*  } 

/*getcnar0; 


mba_init0;  /*  init  vars  etc  */ 

for  (i=0;i<4;i+  +)  /*  make  sure  hshi  shows  proper  motor  positions  */ 

mer_r_mpos(waste) ; 

tty_open(l ,8,8, 1 ,0);  /*  setup  the  serial  port  for  jr3  *! 


tty  outs(l,"DP  S\r");  /*  do  a  clear  buffer  *! 

tty  outs(l,"RO\r");  /*  zero  offsets  */ 
tm  =  time(NULL)  +  2; 
while  (time(NULL)  <  tm); 
while  (tty_in(l)  >  0); 


tty_outs(l,"EA  =  FZ\r");  /*  use  only  z  this  also  starts  hand  shaking*/ 
get Jr3  info(str,9); 

cnts  =1(1.0/133.0)  /  .8380966e-6)  *  2; 

wptr  =  status(l,l,‘‘  INVERSE  KINEMATICS  INFO  ”,75,20); 

done  =  0; 

sprintf(str,"xd  =  %6.31f  Fd  =  %6.2f",xd,Fdesired); 
prints(0,str,  1,5,0); 

while(!done)  /*  until  user  aborts  */ 

ouq)ortb(0x304 , 00) ;  /  *  testing  only ! ! !  */ 

prockeyO; 


prevx  =  x; 
prevy  =  y; 
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/* 

/* 


{)revz  =  z; 
astx  =  mt6_0[0; 
lasty  =  mt6_0[r 
lastz  =  mt6_0[2' 

joint_ang(wptr); 

exot_matO; 


[3] 

[3] 

[3] 


/*  save  prev  xyz  for  indexing  */ 
/*  save  prev  xyz  for  indexing  */ 
I*  save  prev  xyz  for  indexing  */ 

/*  get  joint  angles  from  the  mba  */ 

/*  Computes  all  required  matrix  */ 


/*  elements  *! 

outportb(0x304j02);  /*  testing  only  !!!  */ 

/*  if  (indexing)  I*  set  new  indexing  */ 

X  offset  +=  lastx  -  mt6  0[0][3]; 
y  offset  +=  lasty  -  mt6  0[1][3]; 
z_offset  +=  lastz  -  mt61)[2][3]; 


/* 

/* 

/* 

I* 

I* 

I* 

/♦ 

/* 

/* 

/* 

/* 

*/ 


else 

{ 


} 


X  =  mt6_0[0' 
y  =  mt6_0[l’ 
z  =  mt6_0[2‘ 


/*  calc  new  indexing  position  */ 

+  x_offset;  /*  get  x,y,and  z  -shift  workspace  */ 
+  y_offset; 

-I-  z  offset; 


if(trigr) 

if  (datcnt  =  =  0) 
ptime  =  utimeO; 
data[0]  [datcnt]  x; 

data[l]  [datcnt]  y; 

data[2]  [datcnt]  z; 

datcnt+  +  ; 
if  (datcnt  =  =  2000) 

Uigr  =  0; 

while(l)  /*  wait  proper  interval  before  collecting  */ 

ctime  =  utimeO; 

if  (ptime  -  ctime  >  =  cnts)  /*  done  waiting  */ 
br^;  /*  go  collect  the  data  *7 

} 

if  (j  <  3)  I*  just  not  fast  enough  for  the  task  */ 
exit(pnntf("could  not  sample  fast  enough  \n" 

"ptime  %u  ctime  %uVn", ptime, ctime)); 
ptime  =  ctime; 


getJr3_info(str,21); 
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for(i=0;i<20;i++) 
if  (str[0]  !=  'F') 

strcpy(str,&str[l]); 

else 

if(i<20)’  /*f  found*/  ^  ^ 

sscanf(&str[2] , "  %lf '  ,&zforce);  /*  get  force  from  sensor  */ 


prints(0,str,50,l,0); 
sprintifstr, "  %lf '  ,zforce); 
prints(0,str,50,2,0); 

y  jji  :1c  5j«  }jc  ♦*#*******  * 

force  control  loop 

:|c  :ic  *  :1c  :ic  5|c  :|«  :ic  :|c  sjc  ******  / 


Ferror  =  Fdesired  +  zforce; 
if  ((Ferror  <  ()  &&  Ferror  >  -Ftol)  | 
(Ferror  >  0  &&  Ferror  <  Ftol)) 
Ferror  =  0; 


/* 

/* 

*/ 


X  +  =  flimit((float)(Ferror/Fscale))  *  xd; 


sprintf(str,"x  =  %6.31f  Fscale  =  %6.21f  limit  %6.21f  err  %6.21f’, 

x,Fscale,(double)(flimit((float)(Ferror/Fscale))), 

Ferror); 

prints(0,str,  1,6,0); 

if  (x  <  12)  /*  check  mins  and  maxs  */ 

X  =  12  j 

else  if  (x  >  35.44) 

X  =  35.44; 


if  (z  <  -23)  /*  do  not  allow  crashing  into  floor  */ 

z  =  -23; 

if  (z  <  -28)  /*  do  not  allow  crashing  into  floor  */ 

z  =  -28; 

/*  perform  inverse  kinematics  for  MERLIN  */ 
if  (sinvkin(&wptr,x,y,z,wrist_roll,wrist_flex,tool_roll)  !  =  8) 

X  =  prevx; 
y  =  prevy; 
z  =  prevz; 


if  (datcnt) 

^  fp[0]  =  fopen("xvout.dat","w"); 

fp[l]  =  fopen("yvout.dat'',"w"); 

fp[2]  =  fbpen("zvout.dat'',"w"); 
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for  (i=0;i<3;i++) 

fprintf(Q)[i],'’ %c%s\nTime\nvel  in/sec\nl33\n",'X'  +i,"vout"); 


for  (i=0;i<datcnt-l;i++) 
for(j=0;j<3;j  +  +) 


/*  calc  velocity  at  133  samples  per  sec  */ 
dataD][i]  =  (data[j][i]  -  dataO][i+l])  /  7.5187699e-3; 


for  (i=0;i<datcnt-l;i++) 

{  ■  •  ■■%An",data| 

%  An", data] 
%  An", data] 


} 


fcloseC 

fclosef: 

fcloset: 


close  statusO; 
tty  flush(l); 
tty:flush(2); 
tty_closet21; 
tty  closet  1); 
if  (done  ==  2) 

wind(8,8,"TIMEOUT",'E','  ’, "Timeout  waiting  for  MBA  joint  angles", 
else  if  (done  =  =  3) 

wind(8,8, "TIMEOUT", 'E','  ', "Timeout  waiting fOTJR3  data". 


} 
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DESCRIPTION: 

This  routine  will  get  the  force  and  moment  information 
from  the  jr3  force  torque  sensor 

#include  <stdio.h> 

^include  <conio.h> 

#include  <time.h> 

#include  "merlin. ref" 

static  char  toddl[100]  =  {0}; 

int  getJr3_info(str,amt) 
char  ’“str; 

{ 

char  tbuff[32];  /*  temp  input  buffer  */ 

char  byt; 

intj,i; 

char  pstr[100]; 
long  tm; 

tm  =  time(NULL)  +  2;  /*  time  out  for  data  */  while(tty_cnt(JR3_PORT)  <  amt)  /* 

wait  for  all  data  to  return  from  jr3  */ 

if  (time(NULL)  >  =  tm)  /*  timeout  so  getout  *! 

^  if  (kbhitO  &&  getchO  =  =  27) 

done  =  2; 
return; 

} 

tty  open(JR3  PORT, 8, 8, 1,0);  /*  setup  the  serial  port  for  jr3  *! 

spfmtf(pstr,"%d  Missing  JR3  data",  +  +badcnt); 
prints((),pstr,25, 1 ,0); 

while  (tty  cnt(JR3  PORT))  /*  clear  the  port  */ 
tty_in(lR3  PORT); 

tm  =  time(NULL)  +  2;  /*  setup  time  for  new  request  */ 

tty_outs(JR3_PORT,"DP  S\r");  /*  send  new  request  for  data  */ 
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for(j  =0;j  <  amt;j  +  +)  /*  get  the  data  *! 

if  (tty_cnt(JR3_P0RT))  /*  bad  condition  -  should  not  be  any  leftover  */ 
{  /*  cnaractors  */ 

for(;tty  cnt(JR3  PORT)>0;j  +  +) 
strtn  =  tty  ih(JR3  PORT); 
str|j]  =  NULL; 
prints(0,str,  10, 10,0); 
prints(0,toddl ,  10, 1 1 ,0); 

strcpy(toddl,str); 


tty_outs(JR3_PORT,"DP  S\r");  /*  give  next  request  to  jr3  */ } 
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liHi),*Hc***)tc**************************************************** 

FILENAME:  prockey.c 

FUNCTION  NAME:  prockey 
AUTHOR:  Todd  Mosher 

DATE:  7-30-91 

DESCRIPTION: 

This  will  handle  any  operator  input. 

**J|C*J|CS|«**********>I‘************* ’•'***  **********  ****************/ 

#include  <stdio.h> 

#include  <conio.h> 

#include  <time.h> 

#include  <niath.h> 

#include  "merlin. ref 
extern  int  trigr,datcnt; 

int  prockeyO 

int  i; 

char  str[100]; 
long  tm; 

if  (IkbhitQ)  /*  nothing  to  process  */ 
return; 

i  =  toupper(getch());  /*  esc  aborts  this  program  */ 


if(i  ==  27) 

else  if  (i  =  =  ’S')  /*  stops  system  until  a  q  is  pressed  */  while(toupper(getch())  !  =  'Q') 

else  if  (i  =  =  'T')  /*  trigger  to  save  data  */ 

tfigr  =  1; 
datcnt  =  0; 

else  if  (i  =  =  'R') 

^  wind(8, 8, "RELEASING  LATCH",’!’,'  ’, "Releasing  latch  to  engage  motors",  NULL); 
tty  outs(l,’’RL\r’’);  /*  release  latch  */ 

tm  =  time(NULL)  +  2; 
while  (time(NULL)  <  tm); 
while  (tty  in(l)  >  0); 
close  infdO; 
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tty_outs(l,"DP  S\r");  /*  restart  data  request  */ 

else  if  (i  =  =  ’C) 

^  wind(8,8,"CLR  BUF",’!’,'  '/'Clearing  jr3  serial  buffers", 
'  NIJLU: 


tm  =  time(NULL)  +  2; 
while  (tinie(NULL)  <  tm); 
while  (tty  in(l)  >  0); 
close_infd(); 

tty  outs(l,"DP  S\r'');  /*  restart  data  request  */ 

} 

else  if  (i  =  =  0) 
i  =  getchO; 
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This  will  return  the  number  of  counter 
tics  in  the  5253  counter  timer. 

Each  tic  is  approx  .8380966us  and  the 
counter  is  reset  every  53ms. 

This  is  a  down  counter. 


SjCJiC  ♦  J|C  5ic  5!c  3|C  J|C  9|C  5l«  J|C  *  J|C  Jf:  5i<  *  J|C  ****  sjc  Sff  *  5i«  ***************  / 

#include  <stdio.h> 

<finclude  <conio.h> 

^include  <ctype.h> 


unsigned  int  utime() 

char  arr[5J; 
unsigned  mt  *ptr; 

ptr  =  (unsigned  int  *)arr; 

outportb(0x43,0x00);  /*  latch  current  count  */  .  . 

arr[0]  =  inportb(0x40);  /*  get  data  from  counter  */  arr[l]  =  inportb(0x40); 
retum(ptr[^); 
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FILENAME:  keycont.c 
ROUTINE:  keycontrol 
AUTHOR:  TW  Mosher 

DATE:  3-25-91 

DESCRIPTION: 

This  routine  will  allow  the  user  to  set  any  joint  to  any 
position.  When  done,  this  will  be  calibrated  to  the  new 
zero  position  area. 

#include  <stdio.h> 

^include  <conio.h> 

#include  <niath.h> 

#include  "merlin. ref 

int  key_control(usrfun,uftaram) 

int  (*usrfun)();  /*  pass  in  desired  function  */ 

int  *u^aram;  /*  parameter  for  user  function  */ 

^  char  jnt[6][40]  =  {  "base  -  left/right  ", 

"shoulder  -  up/down  ", 

"elbow  -  up/down  ", 

"wrist  roll  -  left/right", 

"wrist  flex  -  up/down  ", 

"hand  roll  -  left/right " 

char  str[100]; 
int  i,joint  =  0; 
int  k; 
int  wid; 

float  speed  =  .05:  /*  slow  *! 
double  deg  [6]  =  {0}; 

/*  mer  init  servO;  /*  init  servo  values  -  gain  ...  */ 

win3(237),"  ARROW  CONTROL  ",'r,^',"l-6  To  select  joint", 

"F/S  for  fast/slow", 

"Use  arrows  to  move  " , 

"Press  ESC  when  done", 

NULL); 

wid  =  status(5,6,"  INFORMATION  ",60,3);  /*  put  up  a  status 
window  */prints(wid, "Control  speed  is  slow", 31, 1,0); 
prints(wid,jnt[joint]  ,1,1,0);  /*  print  to  status  window  */ 
while  (1)  /*  mlow  user  to  control  the  robot  */ 

{ 

if  (usrfun)  /*  if  user  function  passed  in,  use  it  */ 
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usrfun(ufparam);  /*  call  user  function  */ 

ifOcbhitO) 

k  =  toupper(getchQ);  /*  get  the  key  */ 
switch(lg  r  process  the  key  */ 

case  '1' 
case  '2'. 

/*  joint  selections  */ 


case  '3': 
case  '4' 
case  '5' 
case  '6': 

joint  =  k  -  '1';  /*  convert  to  joint  0-5  ’“/ 
prints(wid,jnt[joint],  1 , 1 ,0);  /*  print  to  status  window  */  break; 
case  'F': 

prints(wid, "Control  speed  is  fast", 31, 1,0); 
speed  =  4; 
break; 
case  'S': 

prints(wid, "Control  speed  is  slow", 3 1,1,0); 

speed  =  .05; 

break; 

case  75:  /*  left  arrow  */ 

if  (joint  =  =  0  1 1  joint  =  =  3  1 1  joint  =  =  5) 
degOoint]  -=  speed; 
break; 

case  77:  /*  right  arrow  */ 

if  (joint  =  =  0  1 1  joint  =  =  3  1 1  joint  =  =  5) 
degljoint]  -t-=  speed; 
breax; 

case  72:  /*  up  arrow  */ 

if  (joint  =  =  2  1 1  joint  =  =  4) 
degljoint]  -=  speed; 
else  irjjoint  =  =  1) 
degljoint]  -l-=  speed; 
break; 

case  80:  /*  down  arrow  */ 

if  (joint  =  =  2  1 1  joint  =  =  4) 
degjjoint]  -i-=  speed; 
else  if(joint  =  =  1) 
degljoint]  -=  sp^; 
break; 

case  27:  /*  esc  */ 

closeinfoO; 
close_status(); 
return; 
default: 
break; 

} 
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if  (degQoint]  >  degmax[joint]) 
degjjoint]  =  degmax[joint]j 
else  if  (degljoint]  <  degmin[]oint]) 
degljoint]  =  degmin|joint]; 

movearm(deg,'D');  /*  move  to  new  encoder  position  */ 
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^jKsKjKJKsk******^!^*******************  ************* 


FILENAME:  calibmer.c 
ROUTINE:  calib_mer 
AUTHOR:  TW  Mosher 

DATE:  3-25-91 

DESCRIPTION: 

This  routine  will  allow  the  user  to  set  any  joint  to  any  position.  When  done,  this  will  be 
c^ibrated  to  the  new  zero  position  area. 

9{(*9ie  He****  ************************************  *****H<**^ 

#include  <stdio.h> 

^include  <conio.h> 

#include  <math.h> 

#include  "merlin. ref 

int  calib_merO 

^  char  str[100]; 

mer_init_serv();  /*  init  servo  values  -  gain  ...  */  key_control(NULL,NULL); 
mer_init_servO; 
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y  >|c  :Je  **  s|e  j|e  ♦  j|e  JK  *♦  *♦♦♦**  9|«  *************  * 

FILENAME;  movearm.c 
PROGRAM:  movearm 

AUTHOR:  TW  Mosher 

DATE:  3-21-91 

DESCRIPTION: 

This  will  command  the  motors  to  the  proper  position 
which  was  passed  in.  The  passed  in  vals  can  be  radians, 
degrees,  or  encoder  counts. 

Parameters 

pointer  to  an  array  of  6  rad  or  deg  (double)  or  encoder  (long) 
char  r  for  rad,  d  for  deg,  e  for  encoder 
note  -  no  range  checking  done  here. 

Returns  0  on  success,  else  non  0  =  error 

-1  =  invalid  data  type  passed  in 
-2  =  can  not  get  control  from  hshi 

){( )|C  SK  9K  3K  ^  ^  ^  ^  ^  ^ ^  ^  ^  SK  )|C  d|c  9ic  sic  3fe  SH  )ic  ^  ^  sic  sic  y 

#include  <stdio.h> 

#include  <conio.h> 

#include  <stdlib.h> 

^include  <math.h> 

#include  "merlin. ref" 

static  long  lastpos[6]; 

int  movearm(data,typ) 
double  *data; 
char  typ; 

{  . 

int  i; 

long  *ldata; 
double  dtmm 
long  encodfo]  =  {0}; 
char  strllOOl; 
char  strl[100]; 
int  maxmov; 

Idata  =  (long  *)data;  /*  make  either  type  the  correct  addr  */ 
typ  =  toupperttyp);  /*  ensure  no  typo  problem  */ 
if  (typ  ==  -D^)  /*  degrees  to  encoders  */ 

^  encod[0]  =  data[0]  *  dtoe[0];  /*  convert  joint  0  */ 
encod[l]  =  data[l1  *  dtoe[l];  /*  convert  joint  1  */ 
encod[2]  =  (datalz]  data[l])  *  dtoe[2];  /*  convert  joint  2  */ 
encod[3]  =  data[3]  *  dtoe[3];  /*  convert  joint  3  */ 
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/*  joints  3,4,&5  are  coupled  together  */ 
dtmp  =  data[4]  *  DTORAD  *  1.2;  /*  1.2  is  ratio  hshi  manual  */ 
encod[4]  =  (data[3]  *  DTORAD  -  dtmp)  *  7639.437; 

I*  ie'i9Mi  is  */ 

/*  ticks  /  rad  hshi  manual  *! 

encod[5]  =  (((data[3]  -  data[5])  *  DTORAD)  +  dtmp)  *  7639.437; 

else  if  (typ  =  =  'R') 

^  encod[0]  =  data[0]  *  rtoe[0];  /*  convert  joint  0  */ 
encod[l]  =  data[l]  *  rtoe[l];  /*  convert  joint  1  */ 

encod[2]  =  (data[2]  -  data[l])  *  rtoe[2]; 

/*  convert  joint  2  */ 

encod[3]  =  data[3]  *  rto^3];  /*  convert  joint  3  */ 

/*  joints  3,4,&5  are  coupled  together  */ 
dtmp  =  data[4]  *  1.2;  I*  1.2  is  ratio  from  hshi  manual  */ 
encod[4]  =  (data[3]  -  dtmp)  ♦  7639.437;  /*  7639.437  is  *! 

/*  ticks  /  rad  from  hshi  manual  */ 
encod[5]  =  (data[3]  -  data[5]  +  dtmp)  *  7639.437; 

else  if  (typ  =  =  'E') 
for  (i=0;i<6;i++) 
encod[i]  =  ldata[i]; 

else 

retum(-l);  /*  bad  type  passed  in  */ 

/*  the  motors  can  attempt  a  30  deg  increment  */ 

/*  when  the  motors  are  on,  but  can  only  attempt  *! 

/*  a  1  degree  increment  when  the  motors  are  off  */ 

maxmov  =  100;  /*  assume  motors  off  -  can  not  move  -  .5  deg  max  *! 
for  (i=0;i < 6;i+  +)  /*  save  prev  val  */ 

if  (lastpos[i]  !  =  hr_mpos- >  axis[i])  /*  has  moved  -  motors  ok  */ 

maxmov  =  8000;  /*  while  tracking  -  30  deg  max  increments  *! 
break; 


for  (i=0;i  <  6;i+  +)  /*  save  prev  motor  position  */ 

lastpos[i]  =  hr_mpos->axis[i]; 
for  (i=0;i <  6;i+  +)  /*  max  motion  per  frame  =  30  deg  *! 

if  (encod[i]  >  hr_mpos-  >  axis[ij  +  maxmov) 

encodfi]  =  hr_mpos-  >  axis[i]  +  maxmov;  /*  limit  max  motion  */ 
else  if  (encod[i]  <  hr  mpos-  >  axis[i]  -  maxmov) 
encod[i]  =  hr  mpos- >  axis[i]  -  maxmov; 


mer  get  ctrlQ;  /*  do  not  update  window  until  we  are  in  control  */ 
strim  =“lSnJLL; 
strl[0]  =  NULL; 
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for  (i =0;i  <  6;i+  +)  /*  update  merlin  joint  areas  */ 

he  mpos->axis[i]  =  encod[i]; 
spnntf(&str[strlen(str)]."  %61d",encod[i]); 
sprintf(&str  1  [strlen(str  1)] , "  % 61d"  ,hr_mpos-  >  axis[i]); 

prints(0,str,  1 , 10,0); 
prints(0,str  1,1, 11,0); 

mer_mov0;  /*  move  merlin  to  new  position  */ 
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^  i(<  ♦♦***♦♦♦  >l«  *♦♦♦»♦♦  >l<  >l<  ik  >l<  **  sN  "It  * 


FILENAME:  mercmds.c 

ROUTINE:  several  low  level  cmds  to  control  the  merlin 
AUTHOR:  TW  Mosher 

DATE:  3-25-91 


DESCRIPTION: 

merjnit  serv  -  does  a  set  servo  parameters  command  -  this  both 

sets  the  servo  stuff  and  resets  the  joint  computers 
mer_mov  -  sets  the  merlin  command  to  move.  -  joint  values 
assumed  to  be  correct  at  this  time 

mer_cmd  -  does  actual  handshaking  with  the  merlin  when  setting  commands. 
mer_r  mpos  -  reads  motor  position  encoders  from  window 
mer_cTose  -  resets  joints  to  0  position 
mer_hshi_exit  -  exits  hshi  pgm 


#include  <stdio.h> 
#include  <conio.h> 
#include  <time.h> 
#include  <math.h> 
#include  "merlin. ref 


int  mer  init  servQ 

{  :  ." 
mt  i; 

for  (i=0;i  <  6;i-l-  -t-)  /*  set  all  servo  parameters  */ 

hc_srv->servo_param[i].max_acc  =  max_srv_acc[i];  hc_srv->servo_param[i].max_vel  = 
max_srv_vel[i]rnc_srv->servo_param[i].gain  =  gain[i]; 

mer_cmd(CMD_SET_P ARAMS);  I*  have  merlin  do  command  */ 


int  mer_r_mpos(mp_encoders) 
long  *mp_encoders; 

int  i; 

mer  cmd(CMD_RD_M_STAT);  /*  this  forces  a  status  update  */ 
for  (i=0;i<6;i+-f)  /*  read  all  motor  position  encoders  */  mp_encoders[i]  = 

hr_mpos->axis[i]; 
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int  mer  rJpos(mp_rad) 
double  ^mp  rad; 

{  . 

inti; 

mer  cmdfCMD  RD  J  ST  AT);  /*  this  forces  a  status  update  */ 

mer“cmd(CMD3RD_FSTAT);  /*  this  forces  a  status  update  */ 

for  (i=0;i  <  6;i+  +)  ,  /*  read  all  joint  angles  */ 

mp_rad[i]  =  hrJpos->axis[i]; 


int  mer  movO  /*  simple  move  command  —  all  joints  */ 

I  I*  have  already  been  set  */  mer_cma(CMD_M_POS) ; 


int  mer_get_ctrlO 
long  tm; 

tm  =  time(NULL)  +  2;  /*  max  wait  for  merlin  robot  */  while(hc_buf-  >buf_stat 

BUF  HOST)  /*  wait  to  gain  control  */ 

if  (time(NULL)  >  tm)  /*  onW  occasionally  check  for  keystroke  */  { 
if  (kbhitO  &&  getchO  ==21)  /*  of  hsni  -  allow  user  abort  break; 

tm  =  time(Nm^L)  +  2; 
printf("  Merlin  is  not  Powered  up  properly\n"); 


int  mer  cmd(cmd) 
int  cmdT 
{ 

mer  get_ctrlO;  /*  make  sure  we  are  in  control  of  buffer  *! 
he  6uf->  command  =  cmd;  /*  set  the  command  word  */ 
he  buf->buf_stat  =  BUF  HSHI;  /*  give  merlin  control  */ 


int  mer_close()  /*  set  robot  to  zero  position  */ 

int  i; 
intj; 

double  angs[6]; 


mer  r  Jpos(angs);  /*  get  current  position 

for  p=0;i<6;i++) 

hc_srv->  servo  param[i]. max  acc  =  1; 
>servo_paramtiJ.max  vel  =1; 
hc_srv->servo_paramri].gain  =  2; 


*/ 

/*  set  all  servos  to  min  */  he  srv- 
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mer  cmd(CMD_SET_P ARAMS);  /*  set  servos  -  this  inits  joint  position  */  for  (i=0;i<6;i++) 
"he  jpos- >  axisFi]  =  -angs[i];/*  move  from  new  0  to  prev  angles  */ 
merJcmd(CMD_J_POS);  /*  have  merlin  move  slow  */  wind(8,8,"WAIT",'E',' 

'/'Press  return  when  MERLIN", 

"Has  returned  to  its  origin", 

NULL); 


int  mer  hshi  exitO 

{  ■  “ 

mer_cmd(CMD_EXIT); 
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yM<***********>l«***3|<*****9|<**********5l<**SK*5H*****5K*3i«***5i«******5|<***5H5K*5!C****  *  *  JH  f^AME  OF 

ROUTINE;  window  init 
NAME  OF  FILE:  window 
CREATION  DATE;  05/22/89 
AUTHOR;  T.  Mosher 


DESCRIPTION: 

This  will  initialize  the  32k  dual  port  ram  that 
is  in  the  VME  chasis. 

REVISIONS 

REV#  DESCRIPTION  INITIALS  DATE 


/*#define  MEM_OFFSET  OxOdOOOOOOO  *! 
#include  "merlin. ref" 

#define  ATCMD  0x0200 


int  window  initO 

{  .  r 

mt  i; 

int  at_cmd  =  ATCMD; 
int  at_stat  =  ATCMD  +  2; 
int  vme_stat  =  ATCMD  +  8: 
int  vme_am  =  ATCMD  +  13; 
int  stat; 


inportb(at_stat); 
inportb(vme  stat); 

stat  =  inporfbfat  stat);  /*  b  added  by  Steve  *! 
if  (stat  &  1) 

printf("Power  is  off  or  cable  is  disconnected.\n"); 
retum(O); 

outportb(at_cmd,  128); 
outportb(vme_am  ,6 1) ; 

stat  =  inportb(at  stat);  /*  b  added  by  Steve  */ 
if  (stat  &  197)  " 

printfC'Setup  status  error: \n"); 
if  (stat  «&  1^) 

printf("interface  parity  error\n"); 
if  (stat  &  64) 
printf("vme  bus  error\n"); 
if  (stat  &  4) 

printf("interface  timeout\n"); 
if  (stat  &  1) 
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printf("power  is  off  or  cable  is  disconnected\n"); 
retum(O); 

c_buf->buf_stat  =  BUF_HOST;  /*  gain  control  of  window  from  merlin  */  retum(l); 
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FILENAME;  forkin.c 
FUNCTION  NAME:  forkinO 
AUTHOR:  Mosher  &  Crabill 

DATE:  22  March  91 


MODIFIED:  3  April  91 

DESCRIPTION:  This  program  will  allow  checkout  of  the  Merlin 

left  arm  forward  kinematics. 


#include  <stdio.h> 
#include  <conio.h> 
^include  <math.h> 


^include  "merlin. ref" 


int  forkin(wid) 
int  *wid; 

{  .  . 

int  i; 

int  wp; 

double  resmat[4]; 
long  motor  encoder[6]; 
char  str[l()0]; 


wp  =  *wid;  /*  get  window  id  */ 

mer  r  mpos(motor_encoder);  /*  read  motor  position  encoders  */ 
prints(wp, "Motor  encoder  values",  14,0,1); 
str[0]  =  0; 
for(i=0;i<6;i++) 

sprintf(«&str[strlen(str)] , "  %  61d  " ,  motor_encoder[i]) ; 
prints(wp,str,l,l,0); 

enc  to  rad(motor  encoder,in_merlin Joint, "MERLIN"); 


in_merlinJoint[2]  -=  1.5707963;  /*  -90  deg  correction  for  elbow  */ 


merltmatO;  /*  Merlin's  tool  roll/global  reference  */ 

/*  transformation  matrix  elements  */ 
prints(wp,  "Joint  angles"  ,18,2,1); 
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str[0]  =  0; 

for  (i=0;i<6;i++) 

sprintf(&str[strlen(str)],"%6.21f  ",in_merlinJoint[i]  *  57.29578);  /*  display  degrees  */ 
prints(wp,str,  1,3,0); 


matmlt(st6_0,gripper_tip,resmat,wp); 

I*  } 

*1 

} 


/*  multiply  tool  roll/  global  ref  */ 

/*  transformation  matrix  by  gripper  */ 
/*  tip  position  matrix  */ 
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yj|e*J|c^j|ej|e*J|ej|<:i<:|c3|eJ|«****3|e*5k**3|<***»KJ|«»l«5|«****************************** 

FILENAME:  sinvkin.c  smart  inverse  kinematics 

FUNCTION  NAME:  sinvkinQ 
AUTHOR:  Mosher  &  Crabill 

DATE:  09  March  91 


MODIFIED: 


DESCRIPTION: 

This  program  will  take  an  xyz  position  and  use  the 
3  main  axis  of  motion  to  move  the  wrist  to  the  position. 
This  then  calls  the  procedure  to  determin  the  wrist  angles 
and  then  finally  does  the  actual  moveing  of  the  robot  to 
the  correct  place. 


:ic  9(c  :|c  »ie  sic  sK  »|e  ^  ^  He  ^  ^  )ic  9{(  ^  sK  3k  9(c  »ic  9ie  Hey 

#include  <stdio.h> 

#include  <conio.h> 

#include  <math.h> 


#include^'^merlmjTef^^^^^^^^^^^^^^ 


*  Calculate  the  k  and  g  function  values 

He 


double  skgfun(indx,typ) 
int  indx; 

char  typ;  /*  g  or  k  */ 

^  double  f[4],k[5],g[5]; 
double  ct3,ca2; 


typ  =  toupper(typ); 


ct3  =  cos(theta[31); 
ca2  =  cos(alpha[2]); 

f[l]  =  a[3]  *  ct3  +  d[4]  *  sin(alpha[3])  *  sin(theta[3])  +  a[2]; 
f[2]  =  a[3]  *  sin(theta[3])  *  ca2  - 

d[4]  *  sin(alpha[3])  *  ct3  *  ca2  - 
d[4l  *  cos(arpha[3])  *  sin(alpha[2])  - 
sin(alpha[2])  ♦  a[3]; 

f[3]  =  a[3]  *  sin(theta[3])  *  sin(alpha[2])  - 
d[4]  *  sinCaJphap])  *  ct3  *  sin(alpha[2])  + 
d[4]  *  cos(a]pha[3])  *  ca2  + 
ca2  *  dpi; 

if  (typ  !  =  'G')  /*  must  be  a  k  function  request  */ 
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{ 


} 


=  ffl 


=  -f[2]; 


=  sqr(f[l])^+«jr,,.^ 


+  sqr(f[3])  +  sqr(a[l])  +  sqr(d[2])  + 

.  ,  [3]; 

k[4]  =  f^3]  *  cos(alpha[l])  +  d[2]  *  cos(alpha[l]); 
retum(k[indx]); 

else 

^  g[l]  =  f[l]  *  cos(theta[2])  -  f[2]  *  sin(theta[2])  +  a[l]; 

"'’I  *  sin(theta[2])  *  cos(alpha[l])  + 

[2]  *  cos(theta[2])  *  cos(alpha[l])  -  f[3]  *  sin(alpha[l])  -sin(alpha[l])  *  d[2]; 


eP]  = 


% 


} 


retum(g[indx]); 


* 

*  Limit  some  motions 

>K 


double  limit(val)  /*  only  for  ranges  from  -pi/2  to  pi/2  */ 
double  val; 

{ 

if  (val  <=  -1.5  *PI) 
retum(2  *  PI  -1-  val); 
if  (val  >=  1.5  ♦PI) 
retum(-2  *  PI  -I-  val); 
retum(vm); 


int  sinvkin(wid ,  x ,  y ,  z ,  wri  st_roll ,  wri  st_flex ,  tool_roll) 
int  *wid; 

double  X ,  y ,  z ,  wri  st_roll ,  wri  st_flex ,  toolroll ; 


double  thetat[2];  /*  temp  theta  */ 
int  i; 
int  wptr; 

wptr  =  *wid; 

/*  sprintf(str,"x,Y,z  =  %lf  %lf  %lf",x,y,z); 
/*  prints(0,str, 0,0,0); 

/*  getcharQ; 

♦/ 
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/* 


r  =  sqr(x)  +  sqr(y)  +  sqr(z); 
if(sqrt(r)  <  18.5) 

retum(prints(wptr, "TOO  CLOSE" ,0,0, 1)); 
else  if  (sqrt(r)  >  36) 
retum(prints(wptr, "TOO  FAR  " ,0,0, 1)); 
prints(wptr,"  ",0,0,0);*/ 

11  =  r  +  hl^artial; 


h[l] 


thetat[0]  =  2  *  atan2((double)(2  *  h[3]  + 

sqrt((double)(4  *  sqr(h[3])  -  4  *  (sqr(h[l])-sqr(h[2]))))),  (double)(2  *  (h[l]  + 

h[2]))); 

thetat[l]  =  2  *  atan2((double)(2  *  h[3]  - 

sqrt(gouble)(4  *  sqr(h[3])  -  4  *  (sqr(h[l])-sqr(h[2]))))),  (double)(2  *  (h[l] 

if  (thetat[l]  <  =  -PI/2.0)  /*  elbow  up  */ 

theta[3]  =  thetat[l]; 
else 

theta[3]  =  thetat[0]; 

/*  sprintf(str,"theta  3  %8.21f",(double)(theta[3]*57.295));*/ 

yjIcjIcHe**********  thCtE  2  **********♦*****/ 

for  (i=l;i<5;i+-f-) 
k[i]  =  ^^fun(i,'K'); 


thetat[0] 


/*  get  k  value  */ 

at^2((double)(;sin(dpha[l])  *  kp]  -I- 

♦ 


=  atan2((doubie)(-sin(aipiiaiij)  k|  ij 
iqrt((double)(sqr(sin(mpha[lJ))  *  sqr(k[i]) 

(sqr(z)  -1-  2  *  z  *  k[4]  -  sqr(sin(alpna[l])) 

SQr(k[2])  -h  sqr(k[4])5))), 

(double)  (z  +  k[4]  +  sin(alpha[l])  *  k[2])); 

thetat[l]  =  2  *  atan2((double)(-sin(alpha[l])  *  k[l]  -sqrt((double)(sqr(sin(alpha[l]))  *  sqr(k[l])  - 
(sqr(z)  -I-  2  *  z  *  k[4]  -  sqr(sin(alpha[ll))  *  sqr(k[2])  +  sqr(k[4]))))), 
(double)(z  -f-  k[4]  +  sin(mpha[l])  *  k[2])); 


thetat[0]  =  limit(thetat[0]); 

if  (thetat[0]  >  =  PI  /  2.0  &&  thetat[0]  <  =  -PI  /  2.0) 
theta[2]  =  thetat[0]; 
else 

theta[2]  =  limit(thetat[l]); 


/*  sprintf|str^'Thete2^%8.2ir,(double2J^the^^^ 

for  (i= 
g[i]  = 
thetat[0] 

thetat[l]  =  2  *  atan2((double)(-(x  *  g[l]  -  y  *  g[2])  -sqrt((double)((sqr(x)  +  sqr(y))  *  (sqr(g[l]) 


[;!■<  3;i-l-  -I-) 
skgfun(i,'G’); 
=  2*  atan2((( 


!tan2((double)(-(x  *  g[l]  -  y  *  g[2])  +  sqrt((double)((sqr(x)  +  sqr(y))  * 
(sqr(g[l])  -I-  sqr(g[2]))))),  (double)(-(y  *  g[l]  +  x  *  g[2]))); 
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sqr(g[2]))))),  (double)(-(y  *  g[l]  +  x  *  g[2]))); 
thetat[0]  =  limit(thetat[0]); 

if  (thetat[0]  <  =  PI  /  2.0  &&  thetat[0]  >  =  -PI  /  2.0) 
theta[l]  =  thetat[0]; 
else 

theta[l]  =  limit(thetat[l]); 

/♦  sprintf(str, "theta  1  %8.2ir,(double)(theta[l]*57.295));  */ 

wrist_anglesO;  /*  calculate  euler  angles  for  MERLIN  wrist  */ 

theta[3]  -I-  =  PI  /  2.0;  /*  correct  90  deg  for  lo  level  drivers  */ 

for  (i  =0;i  <  3;i-l-  -b)  /*  correct  for  indexing  */ 

theta[i]  =  theta[i-t-l]; 

if  (wrist  flex  >  1.4835)  /*  limit  wrist_flex  to  85  deg  */ 

wrist  flex  =  1.4835; 
else  if  (wrist_flex  <  -1.4835) 
wrist  flex  =  -1.4835; 
thetapr  =  wrist_roll; 
theta[4l  =  wrist_flex; 
theta[5]  =  tool_roll; 

movearm(theta,'R’);  I*  move  the  robot  to  the  desired  positions  */  retum(8); 
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^  j|e  He*  *******  5|c  9(«  *****  9!<9!<  JK  9ic  5|c*  **♦*♦♦**♦**♦**  **********  sK  ******  * 


FILENAME:  merinit.c 

FUNCTION  NAME:  merinit 
AUTHOR:  TW  Mosher  ML  Crabill 


DATE:  4/9/91 

MODIFIED: 

DESCRIPTION: 

Assorted  initializations  for  variables. 


************************************************************y 

#include  <stdio.h> 

#include  <conio.h> 

^include  <math.h> 


^include  "  merlin .  ref" 


int  merinitO 

* 


*  init  section  for  inverse  kin  vars 

* 


hl_partial  =  -sqr(a[3])  -  sqr(a[2])  -  sqr(a[l])  -  sqr(d[4])  -sqr(d[3])  -sqr(d[2])  -  2  *  d[4]  *  d[3]  * 


cos(aipnaiJj)  - 

2  *  d[2]  *  (d[4]  *  cos(alpha[3])  *  co 


h[2]  =2*  a[2]  *  a[3]  -  2  *  d[2]  *  d[4]  *  sin, 
sin(alpha[3])  +  2  *  d[2]  *  a[3]  *  sin(alpha[3] 


in|d^ha[3])  * 


* 
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/,tfif^c:lici>c*#itc*************************************************** 


FILENAME:  merltmat.c 

FUNCTION  NAME:  merltmat() 
AUTHOR:  Mosher  &  Crabill 


DATE:  03/22/91 

MODIFIED:  04/02/91 

11:00  am 

DESCRIPTION:  Forward  kinematic  transformation  matrices 

for  Merlin  left  armed  robot. 

iic*ittilt**)it*****************************************************/ 

#include  <stdio.h> 

#include  <conio.h> 

#include  <math.h> 

#include  "merlin.ref 

merltmatO 

double  ctl,nctl,stl,nstl,ct2,nct2,st2,nst2,ct3,nct3,st3,nst3, 

Ct4,nct4,st4,nst4,ct5,nct5,st5,nst5,ct6,nct6,st6,nst6; 

/*  Merlin  Left  Arm  Robot  Matrix[l]  to  transform  from  coordinate  system  at  Waist/Shoulder 
Intersect  back  to  Reference.  *7 

ctl  =  cos(in_merlinjoint[0]); 
nctl  =  -ctl; 

stl  =  sinfin_merlinJoint[0]); 
nstl  =  -stl; 


Stl  0 

[0] 

[0] 

=  ctl; 

stl  0 

[0] 

[1’ 

=  nstl; 

stro 

0 

“2“ 

=  0; 

stro 

a 

[3] 

=  0; 

stro 

r 

[0] 

=  nstl; 

stl  0 

1] 

i 

=  nctl; 

stro 

r 

'2’ 

=  0; 

stl  0 

1 

[3] 

=  0; 

stro 

2] 

[0] 

=  0; 

stro 

’2' 

’1] 

=  0; 

stro 

2] 

'2= 

=  -i; 

stro 

2] 

‘y 

=  0; 

/*  Merlin  Left  Arm  Robot  Matrix[2]  to  transform  from  coordinate  system  at  Shoulder  back  to 
Reference.  */ 

ct2  =  cos(in_merlinJoint[l]); 
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nct2  =  -ct2; 

st2  =  sin(in_merlinJoint[l]); 
nst2  =  -st2; 


st2  0 

[0] 

[0] 

st2"0 

0] 

1' 

st2  0 

[0] 

[2] 

st2  0 

[0] 

[3] 

st2  0 

[1] 

[0] 

st2  0 

r 

1' 

st2  0 

1 

2 

st2  0 

i' 

‘3] 

st2  0 

2 

[0] 

st2  0 

[2] 

1 

st2  0 

2 

2 

st2  0 

2] 

[3] 

=  stl  0 

[0] 

[0] 

=  stl  0 

[0] 

[0] 

=  stl  0 

[0] 

i 

=  stro 

[0] 

1' 

=  stl  0 

1] 

[0] 

II 

5 

r 

[O' 

=  stro 

T 

1' 

=  stro 

r 

1' 

=  stro 

2 

2 

=  stro 

[2] 

"2] 

*ct2: 

*  nst2; 

’*  -12.00; 
*ct2: 

*  nstz; 

’*  -12.00; 

*  nst2; 

*  nct2; 


/*  Merlin  Left  Arm  Robot  Matrix[3]  to  transform  from  coordinate  system  at  Elbow/Wrist  Roll 
intersect  back  to  Reference.  */ 


ct3  =  cos(in_merlinJoint[2]); 
nct3  =  -ct3; 

st3  =  sin(in_merlinJoint[2]); 
nst3  =  -st3; 


st3  0 

[0] 

[0] 

1  =  st2  oroir 

st3  0 

[0] 

[1‘ 

=  st2“0[ 

01 

st3  0 

[0] 

2 

=  -st2‘  0 

r(J] 

st3  0 

[O' 

y 

=  st2  ■Ql 

0] 

st3  0 

'1' 

0' 

=  st2“0[ 

st3  0 

[1] 

1' 

=  st2“0r 

1 

st3  0 

1] 

2 

=  -st2'  0 

st3"0 

[1] 

[3] 

=  st2  "Dl 

st3  0 

2 

[0] 

=  st2  0[ 

2] 

st3  0 

2 

1' 

=  st2"0r 

2] 

st3  0 

2 

'2] 

=  -st2‘  0 

121 

st3  0 

[2] 

[3] 

1  =  st2_‘0[2]I 

0]  * 
01  * 

F 
0 
0 

]p]; 
[5]* 

0 

a 


ct3  -f-  st2  OrORl]  *  nst3; 
nst3  +  st2_0[0][l]  nct3; 


17.3  -t-  st2  0[0][31; 


ct3  -I-  st2  ori 
nst3  -I-  st2_0[ 


[1] 

][1] 


*  nst3; 

*  nct3; 


17.3  +  st2  0[1][3]; 
ct3  -H  st2  0r2][l]  *  nst3; 
nst3  -I-  st2_0[2][l]  *  ncu; 


I]  *  17.3  -I-  st2_0[2][3]; 

/*  Merlin  Left  Arm  Robot  Matrix[4]  to  transform  from  coordinate  system  at  Wrist  Roll/Flex 
intersect  back  to  Reference.  */ 


ct4  =  cos(in_merlinJoint[3]); 
nct4  =  -ct4; 

st4  =  sin(in_merlinJoint[3]); 
nst4  =  -st4; 


st4 

0 

[0] 

[0] 

=  st3 

0 

[0] 

[0] 

st4' 

‘0 

0' 

1' 

=  st3' 

"0 

0' 

[0] 

st4' 

'0 

0' 

2] 

=  st3 

"0 

0' 

[1]; 

st4' 

'0 

[0] 

[3] 

=  st3‘ 

‘0 

[0] 

[1] 

st4' 

■0 

1' 

[0] 

=  st3‘ 

"0 

1] 

[0] 

*  ct4  -I-  st3  0[0][21  *  nst4; 

*  nst4  +  st3_0[0][2]  *  nct4; 

*  17.25  -t-  st3  0[0][3]; 

*  ct4  -I-  st3_0ri][2]  *  nst4; 
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st4  0 

[1] 

[1] 

st3  0 

[1] 

[0] 

st4  0 

'r 

'2 

st3  0 

1' 

‘r 

st4  0 

i 

y 

= 

st3  0 

r 

r 

st4  0 

[0] 

= 

st3  0 

2 

0 

st4  0 

[1] 

= 

st3  0 

2 

[0] 

st4  0 

"2 

2 

= 

st3  0 

2 

[1] 

st4  0 

'2 

[3] 

= 

st3  0 

2 

r 

*  nst4  +  st3_0[l][2]  *  nct4; 

17.25  +  st3  0[1][3]; 

*  ct4  +  st3  0[2l[21  *  nst4; 

*  nst4  +  st3_0[2][2]  *  nct4; 

’*  17.25  +  st3_0[2][3]; 


/*  Merlin  Left  Arm  Robot  Matrix[5]  to  transform  from  coordinate  system  at  Wrist  Roll/Flex 
intersect  back  to  Reference.  */ 


ct5  =  cos(in_merIinJoint[4]); 
nets  =  -ct5; 

st5  =  sin(in_merlinJoint[4]); 
nst5  =  -st5; 


st5  0 

[0] 

[0] 

= 

st4  0 

[0] 

[0] 

st5“0 

[0] 

"r 

= 

st4  0 

[0] 

[0] 

st5  0 

[0] 

2 

= 

st4  0 

[0] 

[1] 

st5  0 

[0] 

[3] 

st4  0 

[0] 

[3] 

st5"0 

[1] 

[0] 

=: 

st4  0 

[1] 

[0] 

st5  0 

r 

1' 

st4  0 

[0] 

st5  0 

T 

2 

st4  0 

r 

r 

st5  0 

r 

[3] 

= 

st4  0 

r 

[3] 

st5  0 

2 

[0] 

= 

st4  0 

[2] 

[0] 

st5  0 

"2 

[1^ 

= 

st4  0 

[2] 

[0] 

o' 

'2 

2 

= 

st4  0 

2 

[1] 

st5_0| 

2 

[3] 

= 

st4  0 

2 

[3] 

*  ct5  +  st4  0[0][21  *  nst5; 

*  nst5  +  st4_0[0][2]  *  nct5; 


’*  ct5  +  st4  0[1][21  *  nst5; 

*  nst5  +  st4_0[l][2]  *  nct5; 


’*  ct5  +  st4  0[21[2]  *  nst5; 

*  nst5  +  st4_0[2][2]  *  nct5; 


/*  Merlin  Left  Arm  Robot  Matrix[61  to  transform  from  coordinate  system  at  Wrist  Flex/Tool  Roll 
intersect  back  to  Reference.  *J 


ct6  =  cos(in_merlinJoint[5]); 
nct6  =  -ct6; 

st6  =  sin  (in  merlin Joint[5]); 
nst6  =  -st6r 


} 


st6_0 

[0] 

[0] 

= 

st5  0 

[0] 

[0] 

st6  0 

[0] 

[1] 

= 

st5  0 

[0] 

[0] 

st6  0 

[0] 

[2] 

st5  0 

[0] 

'1] 

st6  0 

[0] 

[3] 

st5  0 

G 

[3] 

st6  0 

r 

0 

= 

st5  0 

r 

0 

st6“0 

r 

i 

st5  0 

r 

[0] 

st6“0 

r 

2 

st5  0 

r 

[1] 

st6  0 

1“ 

’3] 

= 

st5  0 

r 

[3‘ 

st6"0 

2 

[0] 

st5  0 

2 

[0] 

st6"0 

[2] 

[1] 

=; 

st5  0 

2 

[0] 

st6  0 

[2] 

[2] 

= 

st5"0 

[2] 

[1] 

st6_0 

[2] 

[3] 

= 

st5_0 

[2] 

[3] 

*  ct6  +  st5  0[01[21  *  nst6; 

*  nst6  +  st5_0[0][2]  *  ncto; 

’♦  ct6  +  st5  0[1][2]  *  nst6; 

*  nst6  +  st5_0[l][2]  *  nct6; 

’*  ct6  +  st5  0[2][2]  *  nst6; 

*  nst5  +  stSlOpiP]  *  nct6; 
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FILENAME:  util.c 

FUNCTION  NAME:  several  useful  routines 
AUTHOR:  Todd  Mosher 


DATE:  12-27-90 

DESCRIPTION: 


iiiiHH,ii,*it,H,*****************************************************/ 

#include  <stdio.h> 
include  <conio.h> 

^include  <math.h> 

#include  "merlin. ref 

int  matmlt(tmat, inmat, resmat) 
double  tmat[4][4]; 
double  *inmat; 
double  *resmat; 

register  iht  i,j; 
char  str[100]; 

/*  printsC'T  MATRIX  INFO", 17, 5,1); 
str[0]  =  0; 
for  (i=0;i<4;i-l-+) 


^spri’ntf(^lcstr[strlen(str)],"%10.51f  ",tmat[i][j]);  prints(str,l,6  +  i,0); 


printsC’GRIPPER  T1P",51,5,1); 

*1 


for(i=0;i<4;i-l--l-) 
resinat[i]  =  0; 

^°^^re«natp]’^+^ tmatplp]  *  inmatD];  sprintf(str,"%10.51f  ",resmat[i]); 
/*  prints(wp,str,50,6+i,()); 

*1 

}  ^ 

mt  matmlt33(tmat,inmat, resmat) 
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double  tmat[4][4]; 
double  inmat[4][4]; 
double  resniat[4][4]; 

register  int  i,j,k; 


for  (i=0;i<3;i++) 

^  for  (j=0;j<3;j  +  +) 
resmat[j][i]  =  0; 
for  0=O;j<3;j++) 
fof  (k=0;k<3;k++) 

^  resmatlj][i]  +=  tmat[j][k]  *  inmat[k][i]; 


niatdispfwptr,basy,dispmat) 
double  aispmat[4][4]; 
int  basy; 

{ 

register  int  ij,k; 
char  str[100]; 


for  (i=0;i<3;i++) 

^  str[0]  =  NULL; 
for(j=0;j<4;j  +  +) 

sprintf(&str[strlen(str)] , 


"%7.21f  ",dispmat[i]I3]);  prints(wptr,str,l,i+basy,0); 


} 


} 
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y  :i<  jje  5j<  5|e  5|e  >|e  j|e  >|e  Jie  :|c  3ic  :}c  Jlc  :|c  Jlc  j(e  9je  jjc  :|e  j|e  >jc  iic  *  sic  sic  sjc  sic****  sK  ****  ******  ♦ 

FILENAME:  enctorad.c 

PROGRAM:  enc_to_rad 

AUTHOR:  Monty  Crabill 

DATE:  4-3-91 

DESCRIPTION: 


#include  <stdio.h> 
^include  <conio.h> 


#include  <stdlib.h> 
^include  <math.h> 


#include  "merlin. ref 


int  enc_to_rad(enc_in,rad_out,dev) 
long  *enc_in; 
do^le  *rad  out; 

char  *dev;  /*  valid  dev  =  MBA  or  MERLIN  */ 

{ 

double  dtmp; 


if  (toupper(dev[l])  =  =  'E') 

rad_out[0]  =  enc_in[0]  rtoe[0];  /*  convert  joint  0  */ 
rad_out[l]  =  enc_in[l]  rtoe[l];  /*  convert  joint  1  */ 
rad_out[2]  =  enc_in[2]  rtoe[2]  -I-  rad_out[lJ;  I*  convert  joint  2  */ 
rad_out[3]  =  enc_in[3]  rtoe[3];  /*  convert  joint  3  */ 

/♦joints  3,4,&5  are  coupled  together  ♦/ 
dtmp  =  enc_in[41  /  rtoe[4]; 
rad_out[4]  =  (rad_out[3]  -  dtmp)  *  0.8333; 
rad_out[5]  =  (2  *  rad_out[3]  -  dtmp  -  (enc_in[5]  /  rtoe[5])); 
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/j|ci|c>K***>K:|<i(ci|cs|t>KiK*J|=*>H>t!***********=H****************************** 


FILENAME:  fullsys.c 

FUNCTION  NAME:  full_system 
AUTHOR:  Todd  Mosher  &  Monsy  Crabill 

DATE:  April  23  1991 


DESCRIPTION:  ^ 

This  program  will  allow  a  user  to  control  the  merlin  robot 
using  the  mba  exo-skeleton. 

it!*:^tiliiti*******************************************************/ 

^include  <stdio.h> 

^include  <conio.h> 

^include  <math.h> 


#include  "merlin. ref 

extern  int  stop_print;  /*  this  is  a  var  in  the  tmwin.lib  */ 

/*  used  to  stop  screen  prints  */  unsigned  int  utimeO; 
float  data[3][2000]; 
int  datcnt  =  0; 
int  trigr  =  0; 


full_systemO 


long  tm; 
char  str[100]; 
unsigned  int  ptime, crime; 
int  i,i,k,wptr; 
double  x  =  28; 
double  y  =  18;  /*  y  = 

double  z  =  7; 
double  zforce; 
double  lastx.lasty,lastz; 
long  waste[o]; 
unsigned  int  cnts; 
double  opinp[4]; 
double  resmat[4]; 

FILE  *^[3]; 


0  causes  sqrt  error  —  fix  for  normal  operation 


*/ 


badcnt  =  0;  /*  count  of  bad  comm  packets  */ 

fill  menu(25,8,"ARM?'',  /*  setup  menu  for  selecting  arm  to  use  */ 

■^Rightarm", 

"Left  arm  ", 

NULL); 
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if  (!menuO)  /*  right  arm  selected  */ 

mba_rqst  =  'R'; 

else  /*  left  arm  selected  */ 

mbarqst  =  ’L'; 


j  =0; 
clrscrQ; 

printf(''\n\n  Loading  Exo  Program\n"); 

system(" upload  mba.e68");  I*  load  exo  pgm  */ 


if  ((fp[0]  =  fopen("upload.sta","r"))  ==  NULL) 

printf("Disk  error  or  upload  version  error\n"); 
printf("Press  return  to  continue,  Ctrl  c  to  abort\n"); 
getcharO; 

fscanf(fp[0],"  %d",&i);  /*  get  result  of  upload  to  mba  */ 

fclose(ro[0]); 

if  (i  >  D)  /*  upload  problem  */ 

retum(wind(8,8,"COMM  ERR",'E',’  ',"Can  not  communicate  with  MBA”, NULL)); 


tty_open(MBA_PORT,  10,8, 1 ,0);  /*  setup  the  serial  port  for  mba  */ 

tty_out(MBA_PORT,mba_rqst);  !*  start  handshaking  with  EXO  */ 


clrscrO; 

wptr  =  status(l,l,"  INVERSE  KINEMATICS  INFO  ”,75,20); 


if  (i  =  =  0)  /*  Exo  program  newly  loaded  */ 

^  wind(8,8, "INITIALIZE" ,  'E' ,  ’  ' ,  "Move  all  joints  through  their" , 

"Full  range  of  motion", NULL); 
calib_mba0;  /*  Calibrate  system  */ 


fill  menu(25,8,"WRIST",  /*  setup  the  menu  */ 

"LOCK  WRIST  ON  ",  /*  turn  prints  on/off  for  speed  */ 
"LOCK  WRIST  OFF  ", 

NULL); 

lock_wrist  =  ImenuQ; 


/*  stopprint  =  I;  */ 

/*  while  (IkbhitO)  /*  debug  only  */ 
/♦  ^ 

/*  outportb  (0x300  +  4,0x00); 

/*  outportb  (0x300  +  4,0x02); 

/*  } 

/♦getcharO;  */ 
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mbaJnitO;  /*  init  vars  etc  */ 

for  (i=0;i<4;i+  +)  /*  make  sure  hshi  shows  proper  motor  positions  */ 
mer_r_mpos(waste) ; 

tty_open(JR3_PORT,8,8,l,0);  /*  setup  the  serial  port  for  jr3  */ 

tty  outs(JR3  PORT,  "DP  S\r");  /*  do  a  clear  buffer  *! 

tty“outs(JR3“PORT,  "RO\r");  /*  zero  offsets  */ 

tm  =  time(NULL)  +  2; 

while  (time(NULL)  <  tm);  /*  wait  for  2  seconds  */ 
while  (tty_in(JR3_PORT)  >0);  /*  now  clear  the  buffers  */ 

,"EA  =  FZ\r");  /*  use  only  z  this  starts  hand  shaking*/ 

/  .8380966e-6)  *  2; 


tty  outs(JR3  PORT 
get Jr3  info(str,9); 
cnts  =10.0/133.0) 
done  =  0; 


while(!done) 

outportb(0x304 ,00) ; 
prockeyO; 


/♦  until  user  aborts  */ 

/*  testing  only!!!  */ 

/*  processes  s  (stop)  q(continue)  t(trigger)  esc(exit)  */ 


lastx  =  mt6  0[0][3];  /*  save  prev  xyz  for  indexing  */ 

lasty  =  mt6_0[l][3];  /*  save  prev  xyz  for  indexing  */ 

lastz  =  mt6_0[2][3];  /*  save  prev  xyz  for  indexing  */ 


joint_ang(wptr);  /*  get  joint  angles  from  the  mba  */ 


r  exo  tmatO;  /*  Computes  all  required  matrix  */ 

/*  elements  */ 

matdisp(0,l,mt6  0); 

outportb(0x304,02);  /*  testing  only  !!!  *r 

if  (indexing)  /*  set  new  indexing  */ 

^  X  offset  +=  lastx  -  mt6_0[0][3]; 
y  offset  +=  lasty  -  mt6  0[1][3]; 
z_offset  +=  lastz  -  mtbllPlP]; 

else  /*  calc  new  indexing  position  */ 

^  X  =  mt6  0[0][3]  +  X  offset;  /*  get  x,y,and  z  shift  workspace  */ 
y  =  mt6  0[1][3]  +  y_offset; 
z  =  mt6_0[2][3]  +  z_offset; 

sprintf(str,"xyz  =  %7.21f  %7.21f  %7.21f’,x,y,z); 
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prints(0,str,45,3.0); 


if  (trigr)  /*  set  by  prockey  -  used  to  store  xyz  info  into  a  file  *! 
{  /*  press  t  to  start  the  trigger  */ 

if  (datcnt  =  =  0) 
ptime  =  utimeO; 
data[0][datcnt]  x; 
data[l][datcnt]  y; 
data[2][datcnt]  z; 
datcnt+  +  ; 
if  (datcnt  =  =  2000) 
trigr  =  0; 


J 


I*  collect  only  2000  samples  */ 
/*  wait  proper  interval  before  collecting  */ 


} 


while(l) 
ctime  =  utime(); 

if  (ptime  -  ctime  >  =  cnts)  /*  done  waiting  */ 
break;  /*  go  collect  the  data  *1 

} 

if  (j  <  3)  /*  just  not  fast  enough  for  the  task  */ 

exit(pnntf("could  not  sample  fast  enough  \n" 

"ptime  %u  ctime  %u\n", ptime, ctime)); 
ptime  =  ctime; 


get Jr3  info(stf,21); 
forli=T);i<20;i++) 
if  (str[0]  !=  'F') 

strcpy(str,&str[l]); 

else 

break; 

if  (i  <20)  /*  f  found  */ 

sscanf(«&str[2],"%lf’,&zforce);  /*  get  force  from  sensor  */ 

prints(0,str.50, 1 ,0); 
sprintfistr, ''  %  If’  ,zforce) ; 
prints(0,str,50,2,0); 


if  (x  <  12)  /*  check  mins  and  maxs  */ 

X  =  12; 

else  if  (x  >  35.44) 

X  =  35.44; 

if  (z  <  -23)  /*  do  not  allow  crashing  into  floor  */ 

z  =  -23; 

/*  perform  inverse  kinematics  for  MERLIN  */ 
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sin  vMn  (& wptr,  X ,  y ,  z ,  wri  st_roll ,  wrist_flex ,  tool_roll) ; 
if  (datcnt) 


,  =  fopen("xvout.dat","w"); 
i  =  fopen("yvout.dat","w"); 

^  J  =  fbpenrzvout.dat'', "w"); 

for  (i=0;i<3;i++)  . . 

fjprintf(fp[i],"%c%s\nTime\nvel  in/sec\nl33\n",'X'+i,  vout  ); 

for  (i =0;i  <  datcnt- 1  ;i  +  +) 
for(i=0;j<3;j-H+) 

/*  calc  velocity  at  133  samples  per  sec  */ 
dataDlH  =  (dataOlP]  -  dataD][i+l])  A7.5I87699e-3; 

for  (i =0;i  <  datcnt- 1  ;i  +  +) 


printf(ft) 

[0] 

,"%An",data 

[0][i 

J); 

printf(ft 

,"%f\n",data 

1]D 

J); 

printf(fp 

"1 

,"%f\n",data 

2][i 

i]); 

fclosefft 

fclosecn 

fclosetn 


} 

close  statusO; 
tty_flush(l); 
tty_flush(2); 
tty_close(21; 
tty  close(l); 

if  (done  =  =  2)  . .  ^  .  i  « 

wind(8, 8, ’TIMEOUT", 'E','  '."Timeout  waiting fw  MBA  joint  angles  , 

else  if  (done  =  =  3) 

wind(8, 8, "TIMEOUT", 'E','  '."Timeout  waiting  for  JR3  data". 


} 
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y  )(<#♦♦♦*♦  J)!**********************************************’''***** 


FILENAME:  transp.c 

FUNCTION  NAME:  transpose.c 
AUTHOR:  Todd  Mosher 

DATE:  4-26-91 

DESCRIPTION: 


Jle)|C:|tj|e*#5|C*!|()lCS|tl|C!|«>l«’K*’l‘!(<*****>l<S|“l‘*5|«S|'>l'******^***’l'*****’l‘*************/ 

#include  <stdio.h> 

#include  <conio.h> 

^include  <math.h> 

#include  "merlin.ref" 

int  transpose(inmat,outmat) 
double  inmat[4][4]; 
double  outmat[4][4]; 

register  int  ij; 

for  (i=0;i<4:i-l--f-) 
for  (j=0;j<4;j-l--l-) 

outmat[i][]]  =  inmat[j][i]; 
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^  }|c  ^  9K  ^  ^  Me  ^  sic  3k  aK  }|e  )|(  sK  die  9{c  )ie  9f(  ^  9f(  )|c  ^  )ic  3{c  ^  ^  ^ 

FILENAME:  wristang.c 

FUNCTION  NAME:  wrist_angles 
AUTHOR:  TW  Mosher  and  ML  Crabill 

DATE:  4/26/91 


DESCRIPTION: 

^include  <stdio.h> 
#include  <conio.h> 
^include  <math.h> 


#include  "merlin. ref 


int  wrist_angles() 

double  mt0_5r4][4]; 
double  mata  5  [4]  [4]  =  {0}; 
double  mat53a[4][4]  =  |0|; 
double  mat9  a[4][4]  =  {0}; 
char  str[100J; 
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mr9_5 

[1] 

[1] 

=  0; 

mr9  5 

[1] 

[2] 

=  1; 

mr9  5 

[2' 

0] 

=  0; 

mr9  5 

[2’ 

r 

=  0; 

mr9_5 

[2] 

7 

=  -i 

/*  for  straight  d 

own 

mr9  5 

[0] 

[0] 

=  -1 

mr9  5 

[0] 

r 

=  0 

mr9  5 

[0] 

[2] 

=  0 

mr9  5 

[1] 

[a 

=  0 

mr9"5 

1' 

i 

=  1 

mr9  5 

r 

T 

=  0 

mr9  5 

[O' 

=  0 

mr9  5 

-'1 

r 

=  0 

mr9  5 

"t 

2 

=  -1 

*! 


transpose(mt5_0,  mt0_5) ; 

matmlt33(mtO_5 ,  sr3_0,mata_5) ; 

/*  matrix  to  the  MBAs  inverted  */ 
/*matdisp(0,0,mata_5); 

transpose(mata_5 ,  mat5_a) ; 


/*  transpose  MBA  elbow  matrix  */ 
/*  mult  MERLIN  elbow  rotation  ♦/ 

/*  elbow  rotation  matrix  */ 


/*  now  calc  the  MBAs  9  to  5  matrix  */ 

matmlt33(mat5_a,  mr9_5 ,  mat9_a) ; 
/*matdisp((),5 ,  mat9_a);  */ 


/♦ 

/♦ 

I* 

I* 

I* 

/♦ 

*/ 


wrist  flex  =  atan2(mat9  a[l][l],  .  «  r-^- 

-sqrt((doubfe)(sqr(mat9  a[0][l])  +  sqrfmat9_a[2 
wrist  flex  =  atan2(-sqrt((double)(^r(mat9_a[0][I])  +  sqr( 
"  mat9_ari][l]);  „ 

wrist  roll  =  atan2((double)(mat9  a[2][l]  /  sin(wnst_flex)), 
(double)^mat9  a[0][l]7  sin(wrist  flex))); 
tool  roll  =  atan2((double)(mat9  a[l][21  /  sin(wnst_flex)), 
(double)(mat9  a[l][0]  F sin(wnst  flex))); 
sprintf(str,"wr  %7.21f',(double)(wrist_roll*180.0/3. 14159)); 
prints(0,str,l,ll,0); 

sprinti(str,"wf  %7.21f’,(double)(wrist_flex  *180.0/3.14159)); 
prints(0,str,20, 11,0); 

sprintflstr, "tr  %7.21f '  ,(double)(tool_roll*180.0/3. 14158)); 
prints(0,str,40, 11,0); 


wristang.c 


wrist  flex  =  atan2(mat9  a[l][l], 

1  i  1  _  V  i 


sqrt((double)(sqr(mat9  a[0][l])  +  sqr(mat9_a[2][l]^)); 
ist  flex  =  atan2(sqrt((aouble)(sqr(mat9_a[0J[l])  +  sqr(mat9_a| 
mat9_ari]fl]); 

ist  roll  =  atan2((double)(mat9  a[2][l]  /  sin(wrist_flex)), 
(double)(-niat9_a[0][l]7  sin(wrist_flex))); 


La[2][l]))), 


tool  roll  =  atan2((double)(mat9  a[l][2]  /  sin(wrist  flex)), 
(doublej(mat9'  a[I][0]  /  sin(wrist  flex)))- 
/*  sprintffstr, "wr  %7.21f" ,(double)(wnst_roll*180.0/3. 14159)); 

/*  prints(0,str,  1,10,0); 

/*  sprintnstr, "wf  %^21f',(double)(wrist_flex  *180.0/3.14159)); 

/*  prints(0,str,20,10,0); 

/*  sprintnstr, "tr  %7.21f',(double)(tool_roll*180. 0/3. 14158)); 

/*  prints(0,str,40,10,0); 


} 
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/♦♦♦♦♦♦♦♦♦sK************************************************* 

FILENAME:  merirmat.c 


FUNCTION  NAME:  merlrmatO 
AUTHOR:  Mosher  &  Crabill 

DATE:  04/26/91 


#include  <stdio.h> 

#include  <conio.h> 

^include  <math.h> 

#include  "merlin.ref" 

merlrmatO 

double  ctl  ,nctl  ,stl  ,nstl  ,ct2,nct2,st2,nst2,ct3,nct3,st3,nst3; 


ctl  =  cos(theta[l]); 
nctl  =  -ctl: 
stl  =  sin(tneta[l]); 
nstl  =  -stl; 


srl  0 

[0] 

[0] 

srl  0 

0] 

[1] 

srro 

[1] 

[0] 

srl  0 

r 

V 

srro 

"2: 

'2‘ 

ctl; 

nstl; 

nstl; 

nctl; 

-1; 


/*  Merlin  Left  Arm  Robot  Matrix[2]  to  transform  from  coordinate  system  at  Shoulder  back  to 
Reference.  *! 


ct2  =  cos(theta[2]); 
nct2  =  -ct2; 
st2  =  sin(theta[2]); 
nst2  =  -st2; 


sr2  0 

[0] 

[0] 

=r 

srl  0 

[0] 

[0] 

sr2  0 

[0] 

[1] 

= 

srl  0 

[0] 

[0] 

sr2  0 

[0] 

2: 

= 

srro 

[O’ 

r 

sr2  0 

r 

a 

r= 

srro 

1 

G 

sr2  0 

1 

[1] 

= 

srl  0 

r 

[0] 

sr2  0 

r 

’2“ 

= 

srl  0 

i 

[1] 

sr2  0 

[2] 

0] 

= 

srl  0 

[2] 

'2: 

sr2  0 

[2] 

= 

srl  0 

[2] 

"2’ 

sr2  0 

"2: 

'2' 

O.OT 

♦ct2: 

*  nst2; 

’♦  ct2; 

*  nst2; 

’♦  nst2; 
*nct2; 
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/*  Merlin  Left  Arm  Robot  Matrix[3]  to  transform  from  coordinate  system  at  Elbow/Wrist  Roll 
intersect  back  to  Reference.  */ 

ct3  =  cos(theta[3]); 
nct3  =  -ct3; 
st3  =  sin(theta[3]); 
nst3  =  -st3; 

sr3  0[0][01  =  sr2  0[0][0]  *  ct3  +  sr2  0[0][11  *  nst3; 

sr3  0[0][1]  =  sr2"0[0l[0l  *  nst3  +  srZ  0[0][i]  *  nct3; 

sr3  0[0][2]  =  -srJ  0][2]; 

sr3  0[1][0]  =  sr2  ■D[1][0]  *  ct3  +  sr2  0[1][1]  nst3; 

sr3~0  ‘1][1]  =  sr2"0[l][01  *  nst3  +  sr2  0[1][1]  *  nct3; 

sr3"0[l][2]  =  -srJ  0[I][2]; 

sr3"0[2][0]  =  sr2  T)[2][0]  *  ct3  +  sr2  0[21[1]  nst3; 

sr3"0[2][l1  =  sr2"0[2][0]  *  nst3  +  srZ  0[2][1]  ♦  nct3; 

sr3"0  '2][2’  =  0;  " 
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IHtHHf:********************************************************* 


FILENAME:  jointang.c 

FUNCTION  NAME:  joint_ang 
AUTHOR:  Todd  Mosher 


DATE:  12-27-90 

MODIFIED:  23  Jan  91  by  Montrose  Crabbelly 

DESCRIPTION: 

This  routine  will  get  optical  encoder  information  from 
the  mba  and  then  convert  it  to  joint  angles. 

************************************************************/ 

#include  <stdio.h> 

#include  <conio.h> 

^include  "merlin.ref 

int  joint_ang(wptr) 
int  wptr; 

int  i; 

int  lft_oe[81,rig_oe[8]; 
char  str[150]; 
int  itmp; 

double  exohsrad; 

get  mba  info(lft_oe,rig_oe,mba_rqst);  /*  get  encoder  information  */ 

For  (i=0Ti  <  7;i-l-  +)  /*  calc  angles  of  all  joints  */ 

if  (mba  rqst  =  =  'L') 

exo_l_arm[i]  =  exo  l  hs_rad[i]  -I-  (lft_oe[i]  -  arm_hs_oe[i]) 

^  oe_to_rad[i]; 
else 

/*  exo_r_arm[i]  =  exo_r  hs_rad[i]  +  (arm_hs_oe[i]  -  rig_oe[i]) 

/*  *  oe  to  rad[i]; 

*/ 

exo_r_arm[i]  =  exo_r  hs_rad[i]  +  (rig_oe[i]  -  arm_hs_oe[i]) 

^oe_to_rad[i]; 

/*  dist  from  hard  stop  times  the  */ 

/*  conversion  factor  for  encoder  to  deg  */ 
/*  plus  the  hardstop  position  */ 

prints(wptr,  "Joint  /  Encoder  /  Angle  /  Encoder  to  rad  /  Hardstop(rad)" 

"  /  Hardstop(cnts)",l,12,0); 
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for  (i=0;i<7;i++) 

^  if  (mba  rqst  =  =  'L')  /*  get  proper  display  information  */ 

itmp  =  Ift  oe[i]; 
exo_hs_ra3'  =  exo_l_hs_rad[i]; 

else 

{  .  . 

itmp  =  ng  oe[i]; 

exo  hs  ra(r=  exo  r  hs  rad[i]; 

}  ■  “ 

sprintf(str,"enc  %2d  val  %4d  %7.21f  %7.31f  %7.21f 

%4d  ",  i, itmp, exo  r_arm[i]  *  57.3,  oe_to_rad[i], 
exo_hs_rad,  arm  irs_oe[i]); 
prints(wptr,str,  1 , 13 +i^); 


} 
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^#)|t*>Kl|<)|ei|t)|t*i|<*J|<>l'’l'J|'***>l'*>l'*************************************** 

FILENAME:  mbainit.c 

FUNCTION  NAME:  mba_init() 

AUTHOR:  Todd  Mosher 

DATE:  12-31-90 

MODIFIED:  18  January  91  by  Montrose  Crabbelly 

DESCRIPTION: 

This  routine  will  initialize  any  necessary  vanables. 

^tC^tttiitiiC*******)^^***)^!*******************************************/ 

#include  <stdio.h> 

#include  <conio.h> 

^include  "merlin. ref 


int  mba  initf) 

{ 

FILE  *fp; 
int  i; 


} 


if  ((fp  =  fopen("mba.cfg","r"))  ==  NULL) 

^  tty_close(MBA_PORT); 
wn  exitO; 

exif(wind(9,9,"FILE  NOT  FOUND", 'E', • 

"MBA  configuration  file  missing  , 
"Press  return!", 

NULL)); 

else 


for  (i=0;i<8;i-H-H) 
fscanf(fp,"  %d",&arm_hs_oe[i]); 
fclose(ro); 
mt2  0[0][2]  0; 

mt2  0[0][3]  13; 

mt2“0[l][2]  -.1564; 

mt2~0[l][3]  -(11  -I-  14  *  .1564); 

mt2“0[2][2]  -.9877; 

mt2“0[2][3]  -6.315  *  (11  +  14  *  .1564) 

(11 -I-  12  *  .1584)  /  .1584; 
x_offset  =  initx_offset; 
y_offset  =  inity_offset; 
z_offset  =  initz_offset; 


"Calibration  should  create  this  file" , 
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/♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦♦^♦♦♦♦JltiKsKimK********************** 

FILENAME:  rexotmat.c 


FUNCTION  NAME:  r_exo_tmatO 
AUTHOR:  Monty  Crabill 

DATE:  06/18/92 


MODIFIED: 

DESCRIPTION:  .  .  , 

Using  MERLIN  joint  angle  information,  this  creats  the  T  matnx  for  the  wrist 
position  and  the  rotation  matrix  for  the  gripper  with  respect  to  the  elbow. 
Tne  following  method  for  calculating  a  final  tmatrix  is  almost  as  fast  as 
crunching  the  matrixs  by  hand  and  coming  up  with 
an  equation  (we  know,  we  tested  it).  However,  this  method  is  far  easier  to 
modify,  maintain,  and  read. 

Jjc  ♦♦♦♦  j|<  5|<  ♦♦♦♦♦♦  JK  ♦♦♦♦♦♦♦♦♦  Me  *♦**♦**♦***♦♦*♦****♦*  ♦***  s|<  / 

iWnclude  <stdio.h> 

^include  <conio.h> 
include  <math.h> 
include  "merlin. ref 

/*  double  Ct2,st2,nst2,ct3,nct3,st3,nst3,ct4,nct4,st4,nst4, 

Ct5,nct5,st5,nst5,ct6,nct6,st6,nst6,ct7,st7,nst7, 

ct8,nct8,st8,nst8; 

/*  nct7  is  not  used  */ 

/*  used  in  tmat()  to  allow  computation  of  *! 

I*  cos(*K)  &  sin(*tX)  only  once  */ 


r  exo  tmatO 

r  ■ 

/*  MBA  Exo  Right  Matrix[2]  to  transform  from  coordinate  system  at  */ 
/*  Right  Shoulder/ Azimuth  Elevation  Intersect  back  to  Reference.  */ 


ct2  =  cos(exo_r_arm[0]); 
st2  =  sin(exo_r_arm[0]); 
nst2  =  -st2; 


mt2  0| 
mt2  0| 
/♦  mt2  OrO 
/*  mt2  0[0 
mO  0 


0][0]  =  ct2: 

0][1]  =  nst2; 

=  0; 

3]  =  li; 

][1]  =  -.9877  *  ct2; 


computed  once  in  mbainit.c  */ 

"  */  mt2_0[l][0]  =  -.9877  *  st2; 


140 


rexotmat.c 


*/  mt2_0[2][0]  =  .1564  *  st2; 


+  (11  +  12  *  .1584)  /  .1584;  *! 


/*  MBA  Exo  Right  Matrix[3]  to  transform  from  coordinate  system  at  Right  Shoulder  Elevation/ 
Upper  Arm  Roll  Intersect  back  to  Reference.  */ 

ct3  =  cos(exo_r_arm[l]); 
nets  =  -c6; 

st3  =  sin(exo_r_arm[l]); 
nst3  =  -st3; 
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=  -.03r*  inf2 
=  mt2  0[2]| 

=  mt2~0[2l 
=  -mX'I  0[2 
=  -mt2~0[2][ 


OrOlpl  *  st3; 
2_0[0][2]  *  ct3; 


*  ct3  +  mt2  OrO] 

*  nst3  +  mt? 

j’*  .032  +  13; 

*  ct3  +  mt2  0[1][2]  ♦  st3; 

*  nst3  +  mt2_0[l][2]  *  cu; 

*  nst3  +  mt2_0[2][2]  ct3; 
]’*  .032  +  mt2_0[2][3]; 


/♦ 


MBA  Exo  Right  Matrix[4]  to  transform  from  coordinate  system  at  Right  Upper  Arm  Roll/  Elbow 
Intersect  back  to  Reference.  */ 

ct4  =  cos(exo_r_arm[2]); 
nct4  =  -ct4; 

st4  =  sin(exo_r_arm[2]); 
nst4  =  -st4; 

*  ct4  +  mt3  0[0][2]  *  nst4; 
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*  nst4  +  mtJ 


nct4; 


*\5  +  mt3  0[0][31; 

*  ct4  +  mtj  orlim 

*  nst4  +  mt3’ 


5111 


*  nst4; 

]  *  nct4; 


*  15  +  mt3  0[1 

*  ct4  +  mtJ 

*  nst4  +  mtJ 


0[2][2l  *  nst4; 

L0[2][2]  * 


nct4; 
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nit4  0[2][2]  mt3  0[2][1]; 

mtC0[2][3]  mt3I0[2][l]  *  15  +  nit3_0[2][3]; 


/*  MBA  Exo  Right  Matrix[5]  to  transform  from  coordinate  system  at  Right  Elbow  and  Lower 
Arm  Roll  Intersect  back  to  Reference.  */ 


ct5  =  cos(exo_r_arm[3]); 
nets  =  -ct5; 

st5  =  sin(exo_r_arm[3]); 
nst5  =  -st5; 


mt5  0 

[0] 

[0] 

mt5  P 

r 

mt5  P 

[0] 

2 

mt5  P 

[0] 

[3] 

mt5  P 

[1] 

[0] 

mt5  0 

[1] 

[1] 

mt5  0 

r 

2 

mt5  P 

[1] 

[3] 

mt5  0 

'2 

[0] 

mt5  0 

2^ 

r 

mt5  0 

2 

2 

mt5  0 

2 

3] 

mt4  0[0 
mt4  0[0 
-  mt4  0[0]fl]; 
mt4_0t0][3]; 


0]  ♦ 
0]  ♦ 
][!]; 


mt4"0[l] 
mt4~0ri] 

-  mf4  Of] 
mt4  0[L 
mt4_0[2 
mt4  0[2 

-  mt4  b[2][l]; 
mt4  0I2][3]; 


0]  *  ct5  +  mt4  OrO 
p]  nst5  +  mt4_0[i 


|][: 


♦  ct5  +  mt4  Oil 
nst5  +  mt?_0[ 


k 

Q  *  ct5  +  mt4  0 
P]  nst5  +  mt? 


][2] 


*  st5; 
1]  *  ct5; 


*  st5; 
*ct5; 


*  st5; 

]  ♦  ct5; 


/*  MBA  Exo  Right  Matrix[6]  to  transform  from  coordinate  system  at  Right 
Lower  Arm  Roll  &  Wrist  Radial  Intersect  back  to  Reference.  */ 


/*  ct6  =  cos(exo_r_arm[4]); 

/*  nct6  =  -ct6; 

/*  st6  =  sin(exo_r_arm[4]); 

/*  nst6  =  -st6; 


/*  some  element  calcs  have  been  eliminated  since  */ 
/*  only  the  position  info  from  tmat  6  is  used  */ 


/* 

mt6  0 

[0] 

[0] 

1* 

mt6~0 

[0] 

r 

1* 

mt6  0 

X 

*/ 

mt6 

0| 

/* 

mt6  0 

[1] 

1* 

mt6“P 

[1] 

1* 

mt6“0 

1' 

[2] 

*/ 

mt6 

0| 

1* 

mt6  0 

[2] 

[0] 

/* 

mt6~0 

2 

[1] 

/* 

mt6“0 

2 

[2 

=  mt5  0[0: 
=  mt5  Pto 
=  -mt510[P 


0]  *  ct6  +  mt5  0[0][2]  ♦  st6; 
“■  *  nst6  +  mt5_P[P][2]  *  ct6; 


0]  * 
[1]; 


n 


*  16  +  mt5 
+  mt5  P[1 


q[11[0l  *  nst6  +  mt5_0[l][2]  ♦  ct6; 


*/ 


1][3]  =  -mt5  0[1][11  ♦  16  +  mt5  0[1][3]; 
=  mt5  0[2]ID]  *  ct6  +  mt5  0[2jr2]  ♦  st^; 
=  mt5"0r21[0]  ♦  ' 

=  -mt5'  0[2][1]; 


*  nst6  +  mt5_0[2][2]  *  ct6; 
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mt6_0[2][3]  =  -mt5_0[2][l]  *  16  +  mt5_0[2][3]; 

/*  MBA  Exo  Right  Matrix[7]  to  transform  from  coordinate  system  at  Right 
Wrist  Radial  &  Wrist  Flex  Intersect  back  to  Reference.  */ 

/♦  no  longer  needed  */ 


/*  ct7  =  cos(exo_r_arm[5]); 

/*  nct7  =  -ct7; 

/*  st7  =  sin(exo_r_arm[5]); 

/*  nst7  =  -st7; 


!* 

mt7  0 

[0] 

[0] 

=  mt6  0 

[0] 

[0] 

1* 

mt7“0 

[0] 

[1] 

=  mt6  0 

[0] 

[0] 

1* 

mt7“0 

[0] 

"t 

=  mt6  0 

[Oj 

/* 

mt7“0 

G 

=  -mt6’  1 

)[6 

[1 

1* 

mt7“0 

r 

[0] 

=  mt6  T) 

[1] 

1* 

mt7~0 

r 

r 

=  mt6  0 

"V 

1* 

mt7“0 

V 

=  mt6  0 

ni 

r 

/* 

mt7~0 

[1] 

[3] 

=  -mt6'  Or 

[1 

/* 

mt7“0 

‘2] 

[0] 

=  mt6 _J)[z 

/* 

mt7“0 

i: 

[1] 

=  mt6  0[21 

[0] 

1* 

mt7“0 

'I 

=  mt6  012 

1 

1* 

•if  i 

mtT^O 

[3] 

j  = 

[1 

*  ct7  +  mt6  0[01[21  *  nst7‘ 

*  nst7  +  mt6_0[0][2]  *  net/; 


i  *  .160  +  mt6  01 

*  ct7  +  mt6  on 

*  nst7  +  mt6_0[ 


:0][3];  ^ 

*  nst7‘ 

]  *  nct7; 


]  ♦  .160  +  mt6  0[1][3]; 

*  ct7  +  mt6  0[2][2]  ♦  nst7; 

L0[2][2]  ^  ' 


*  nst7  +  mt6 


*  nct7; 


]  *  .160  +  mt6_0[2][3]; 


/*  MBA  Exo  Right  Matrix[8]  to  transform  from  coordinate  system  at  Right 
Wrist  Radial  &  Wrist  Flex  Intersect  back  to  Reference.  */ 

/*  no  longer  needed  */ 

/*  ct8  =  cos(exo_r_arm[6]); 

/*  nct8  =  -ct8; 

/*  st8  =  sin(exo  r_arm[6]); 

/*  nst8  =  -st8; 


/* 

mt8  0 

[0] 

[0] 

=: 

mt7  0 

[0] 

[0] 

*  ct8  +  mt7  0[0][2]  *  nst8; 

/* 

mt8  0 

[0] 

i 

mtr"0 

[0] 

[0] 

*  nst8  +  mt7_0[0][2]  *  nets; 

1* 

mt8  0 

[0] 

2 

== 

mt7~0 

[0] 

r 

j 

1* 

mt8  0 

[0] 

[3] 

= 

mt7“0 

[0] 

[3] 

9 

/* 

mt8  0 

[1] 

[0] 

= 

mt7"0 

[1] 

[0] 

*  ct8  +  mt7  0[1][21  *  nst8; 

*  nst8  +  mt7_0[l][2]  *  nct8; 

/* 

mt8  0 

r 

[1] 

= 

mt7“0 

[1] 

[0] 

1* 

mt8  0 

r 

"2 

= 

mt7“0 

‘1] 

9 

1* 

mt8  0 

r 

[3] 

= 

mt7“0 

r 

‘3’ 

9 

/* 

mt8  0 

"I 

= 

mt7”0 

'2 

G 

♦  ct8  +  mt7  0[2][2]  *  nst8; 

*  nst8  +  mt7_0[2][2]  *  nct8; 

1* 

mt8  0 

"2 

r 

= 

mtT^O 

2 

[0] 

I* 

mt8  0 

"2 

'2 

= 

mt7“0 

[2] 

'!] 

9 

/* 

*/ 

mt8  0 

"2 

[3] 

mt7“0 

[2] 

[3] 
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^  9|<)io(c  sic  )|c  He  9K  sfe  3|e  aK 

*  rotation  matrix  from  the  wrist  to  the  elbow 

* 

He  He  He  He  He  He  lie  ^  ^  He  He  ^  9ie  He  )|e  )j<  ^  He  )ie  He  He  He  He  He  3ie  He  He  He  ^ 


ct6  =  cos(exo_r_arm[4]); 
st6  =  sin(exo_r_arm[4]); 

/*  matrix  to  describe  wrist  roll  to  the  elbow  */ 


mr6  5 

[0] 

[0] 

mr6“5 

0] 

[1] 

mr6”5 

1] 

’2] 

mr6~5 

[2] 

[0] 

mr6“5 

'2' 

r 

=  ct6: 
=  -sto; 

=  -st6; 
=  -ct6; 


/*  matrix  to  describe  wrist  flex  to  the  elbow  */ 


ct7  =  cos(exo_r_arm^]); 
st7  =  sin(exo_r_arm[5]); 


mr7  5 

[0] 

[0] 

= 

mr6  5 

[0] 

[0 

mr7”5 

[0] 

[1] 

= 

mr6~5 

[0] 

[O' 

mrT^SI 

0] 

[2] 

= 

mr5“5 

[0] 

[1' 

mr7”5 

[1] 

[0] 

= 

st7; 

mr7”5 

[1] 

[1] 

= 

ct7; 

mr7“5 

U 

0; 

mr7”5 

'2! 

0 

= 

mr6  5 

[2] 

[0] 

mr7“5 

"I 

r 

= 

mr6“5 

2’ 

[0] 

mr7“5 

[2] 

[2^ 

= 

mr6“5 

[2] 

[1] 

*ct7; 
*  -st7; 


*ct7; 
*  -st7; 


/*  matrix  to  describe  tool  roll  to  the  elbow  */ 


ct8  =  cos(exo_r_arm[6]); 
st8  =  sin(exo_r_arm[6]); 


mr8  5 

[0] 

[0] 

mr7  5 

[0] 

[0] 

mr8  5 

[0] 

=: 

mr^5| 

[0] 

[0] 

mr8  5 

[0] 

[2] 

= 

mr7~5 

[0] 

[1] 

mr8  5 

1 

O' 

mr7“5 

1' 

O' 

mr8  5 

1] 

'1 

= 

mr7“5 

[1] 

[0] 

mr8  5 

[1] 

[2] 

= 

mr7“5 

[1] 

[1] 

mr8  5 

2' 

0' 

= 

mr7“5 

[2' 

[O' 

mr8  5 

’2] 

[1] 

= 

mr7“5 

[2] 

[0] 

mr8  5 

2' 

7 

= 

mr7“5 

2' 

'1' 

*  ct8  +  mr7  5[01[21  ♦  -st8; 

*  -st8  +  mr715[0][2]  *  -ct8; 

’♦  ct8: 

*  -st8; 

’*  ct8  +  mr7  5r2][2]  ♦  -st8; 

*  -st8  +  mr715[2][2]  *  -ct8; 


/*  matrix  to  orient  the  frame  8  the  same  as  the  merlin  gripper  */ 


mr9_5[0][0]  =  mr8_5[0][2]; 
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mr9  5 

[0] 

[1] 

= 

-mrS  5| 

[0] 

|[0] 

mr9  5 

[0] 

X 

= 

-mr8  5| 

o: 

mr9  5 

1] 

0' 

== 

mr8  3[ 

mr9  5 

V 

r 

= 

-mrS"  5| 

[0] 

mr9  5 
mr9_5 

r 

-'X 

[2] 

[0] 

= 

-mr8  5 
mr8  3[! 

mr9  5 

[2] 

[1] 

= 

-mrS"  5| 

[2] 

[0] 

mr9_5 

"1 

X 

= 

-mr8  5| 

[2; 

[1] 

} 


145 


calib.c 


FILENAME:  calib.c 

FUNCTION  NAME:  calib_mba() 

AUTHOR:  Todd  Mosher 

DATE:  12-31-90 

DESCRIPTION: 

This  routine  will  instuct  the  user  how  to  calibrate  the 
mba. 


#include  <stdio.h> 
#include  <conio.h> 
#include  "merlin. ref 


extern  int  tty  cnt; 
char  caltxtrSlpS]  = 

{  {’^'shoulder  azimuth"}, 
{"shoulder  elevation"), 
["upper  arm  roll"), 
"elbow  flex"), 

"lower  arm  roll"), 
"wrist  roll"), 

"wrist  flex"), 

}■;  ’ 


int  calib  mba() 

{ 

FILE  *fp; 
int  i,j; 

char  strllOOl; 
char  str2[I00]; 
int  lft[8],right[8]; 
int  res[8]; 

wind(8,2,"INFORMATION",'r,'  ', 

"All  rotations  are  clock  wise.  Look  down  the  ", 
"optical  encoder  shaft  into  the  housing  to  ", 
"determine  the  proper  direction", NULL); 


for  (i=0;i<7;i-l-+)  /*  for  each  joint  */ 

sprintf(str, "Please  rotate  the  %s",cal_txt[i]); 
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if  (mba  rqst  =  =  'R')  /*  display  proper  arm  request  */  sprintf(str2,"to  the  hardstop 

(right  arm)."); 
else 

sprintf(str2,"to  the  hardstop  (left  arm)."); 


} 


wind(10, 8,  "INSTRUCTIONS" ,  'E' , ’  ' , 

str, 

str2, 

"Press  enter  when  this  is  done. " , 

NULL)* 

get  mba  info(lft, right, mba  rqst);  I*  this  gets  data  from  old  data  request  *! 
germba“info(lft,right,mba_rqst);  /*  this  gets  current  data  */ 


if  (mba  rqst  =  =  'L') 
resti]  =  lft[i]; 
else 

^  res[i]  =  right[i]; 
close_info(); 


/*  save  proper  results  */ 
/*  save  result  in  res  */ 

/*  save  result  in  res  */ 


fp  =  fopen("mba.cfg","w");  /*  save  the  current  info  */ 
for(i=0;i<8;i++) 

fprintf(fp,"%d\n",res[i]); 

fclose(fp); 
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^jjcj|e3jc)|c3|c:ic5K5|e**5i<5!<*JK9K5|es|<*%5|€5K5|c*********  *********************  ******** 


FILENAME:  getmba.c 

FUNCTION  NAME:  get_mba_info() 
AUTHOR:  Todd  Mosher 

DATE:  12-31-90 


DESCRIPTION: 

This  routine  will  get  the  optical  encoder  information 
from  the  mba 


****)iC********9{C3i<9}C  *************************  *******************^ 


#include  <stdio.h> 
^include  <conio.h> 
#include  <time.h> 
#include  "merlin.ref 


extern  int  trigr,datcnt; 


int  get_mba  info(lft_oe,rig_oe,typ) 
int*lft  oe,’'^ig_oe; 

int  typ;  /*  Left  Right  or  H/Both  */ 

^  char  tbuff[32];  /*  temp  input  buffer  */ 
char  byt; 
intU; 

mtbytecnt  =  17; 
char  str[100]; 
unsigned  int  tm; 


bytecnt  =  33; 


-H-) 


tm  =  utimeO; 

while(tty_cnt(MBA_PORT)  <  bytecnt)  /*  wait  for  all  data  to  return  from  mba  */ 
if  (tm  -  UtimeO  >  =  25000)  /*  timeout  so  getout  */ 

^  if  (kbhitO  &&  getchO  =  =  27) 

done  =  2; 
return; 

} 

tty  open(MBA  PORT,  10,8, 1,0);  /*  setup  the  serial  port  for  mba  */ 

spnntf(str,"%<rMissing  MBA  data",-l--l-badcnt); 
prints(0,str,25,l,0); 
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/♦ 


*/ 

} 


while  (tty  cnt(MBA  PORT))  /*  clear  the  port  */ 

"  tty_in(MBA  PORT); 

tty_out(MBA_PORT,typ);  /*  send  out  new  request  for  data  */ 
tm  =  utime();  /*  setup  time  for  new  request  */ 


}. 


/*  turn  off  indexing  *! 

I*  get  leading  byte  */ 
/*  index  reset  */ 


/*  turn  on  indexing  */ 


indexing  =  0; 
byt  =  tty  in(MBA  PORT); 
if'(byt  ==  'R')  ■ 

{ 

x_offset  =  initxoffset; 
y_offset  =  inity_offset; 
z_offset  =  initz_offset; 

else  if  (byt  =  =  T) 
indexing  =  1; 

else  if  (byt !  =  'H'  &&  byt !  =  'Z')  /*  h,i,r,z  are  valid  header  bytes  */ 

{  /*  z  is  halt  button  */ 

while(tty  cnt(MBA  PORT))  I*  clear  the  port  */ 
tty  in(MBA  PORT); 

sprintffstr, "  %d' Bad  header  found" ,  +  +badcnt); 
prints(0,str,25, 1 ,0); 

if(typ  ==  'H'  M  typ  ==  'B') 
rbr  (j  =31;j  >  =0;j")  /*  get  the  data  */ 

tbuffm  =  tty  in(MBA  PORT); 
else  if  (typ  =  =  T') 
for(j=31;j>=16;j-) 

tbuffD]  =  tty  in(MBA_PORT); 
else  if  (typ  =  =  ’^R') 
forC  =  15;j>=0;j-) 

tbuffm  =  tty  in(MBA_PORT); 

ttv  out(MBA  PORT, typ);  /*  give  next  request  to  mba  */ 

it  (byt  =  =  'Z')  /*  when  in  halt,  do  not  update  numbers  */  return; 

if (t^  ==  'B'  1 1  typ  ==  'H'  11  typ  ==  'R') 
for  (j  =7;j  >  =0;j-)  /*  put  highbyte,lowbyte  together  */ 

rig  6en]=  ((tbuff[2*j  +  l]<  <8)  &  OxFFOO)  1  (tbuff[2=^j]  &  OxOOFF); 
if  (typ®==%'  n  tvD  ==  'H'  1 1  tvp  ==  'L') 


f  ( ^  15  * ^  s'*  )  '  '  ^ 

lfT_oei-8]^(('tbuff[2*j  +  l]<  <8)  &  OxFFOO)  1  (tbuff[2*j]  «fe  Ox(X)FF) 


printft"%5d  %5d  %5d  %5d\n",rig_oe[0].rig_oe[l],rig_oe[2],rig_oe[3]);  printf("%5d  %5d 
%5d  %5d\n",rig_oe[4],rig_oe[5],ng_oe[o],ng_oe[7]); 
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/* 

TTYOPEN.C 
Task  25,  Robotics 
KL  Johnston,  SRL 

Functions  used  by  TTY68K  to  initialize  and  restore  IBM-PC  serial  ports, 
A  serial  input  pattern  matching  function  is  also  available. 

*1 


^include  <bios.h> 
#include  <dos.h> 
^include  <stdio.h> 


#define  ISTOP 
jj^define  ■2STOP 
#define  "TBITS 
#define  "8BITS 
#defmeBASEl 
#define  BASE2 
#define  DLAB 
#define  DTR 
#define  IRQl 
#define  IRQ2 
iS'define  OUTl 
#define  OUT2 
ijfdefine  RTS 
/^fdefine  VECTORl 
#define  VECTOR2 
int  port  =  1 : 
intoaud  =  8; 
int  dbits  =  8; 
int  sbits  =  1; 
int  parity  =  0;  ^  ^ 

int  baud_table[12][5]  = 

^  '  50,9, 

110,  4, 

150,  3, 

300,  1, 

600,  0, 

1200,  0, 

2400,  0, 

4800,  0, 

9600,  0, 

19200,  0, 

38400,  0, 

56000,  0, 


0x00 
0x04 
0x02 
0x03 
0x3F8 
0x2E8 
0x80 
0x01 
0x10 

0x20  /*0x08  */ 

0x04 
0x08 
0x02 
OxOC 

OxOD  /*  OxOB  */ 

/*  com  port ...  default  coml  */ 

/*  index  into  baud  table. . .  default  9600  */ 
/*  data  bits...  default  8  */ 

/*  stop  bits. , .  default  1  */ 

/*  parity.,  default  none  */ 

/*  baud  table  for  serial  io  stuff  */ 
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void  interrupt  tty l_isrf void); 
void  interrupt  tty2_isr(void); 
extern  int  ttyl  base,  ttyl_error,  ttyl_qfull,ttyl_cnt; 
extern  int  tty2njase,  tty2_error,  tty2_qfull,tty2_cnt; 


static  int  irq_level,  vector; 
static  void  interrupt  (*old_isr)0; 


^  y  open  *^^*****^*’^^***************************/ 

/♦♦♦♦♦:ic*3|c:K****************i»J^**>*******5K******************************  5|C5|«JK :(€**/  void 
tt^_open(int  port, int  baud,int  dbits,int  sbits,int  parity) 


/ 


*/ 

{ 


Initializes  serial  port  and  sets  tty_isr  (see  TTYISR.ASM)  as  interrupt 
service  routine. 

port  =  1  -  2  for  COMl  or  COM2  port, 
baud  =0-11,  index  into  baud_table  array, 
dbits  =7-8,  number  of  data  bits/byte. 
sbits  =1-2,  number  of  stop  bits/byte. 

parity  =  EVEN_PARITY,  ODD  PARITY  or  NO_PARITY  (see  TTYOPEN.H). 
This  routine  is  not  intended  to  befool  proof,  it  assumes  valid  arguments 
and  returns  no  status. 


int  w_len,  n_stop; 
int  ttybase; 


if  (port  =  =  1) 

ttyl  base 
tty_5ase 
vector 
irq_level 

else 

tty2  base 
tty_5ase 
vector 
irq_level 

if  (dbits  =  = 
w_len  = 
else 

w  len  = 
if  (sbits  =  =  T) 


=  BASEl; 

=  BASEl; 

=  VECTORl; 
=  IRQ1; 


=  BASE2; 

=  BASE2; 

=  VECTOR2; 
=  IRQ2; 


BITS; 

8BITS; 


151 


ttyopen.c 


n_stop  =  _1ST0P; 
dsc 

n_stop  =  _2STOP; 
old  isr  =  getvect(vector); 
if  ^rt  =  =  1) 

setvect(vector,ttyl_isr);  /*  Establish  8259  handler  for  tty  IRQ  level  */  else 
setvect(vector,tty2_isr);  /*  Establish  8259  handler  for  tty  IRQ  level  */ 


} 


DTR);  /*  Disable  any  pending  interrupts  *! 


parity); 
b(ttY  bas 


disableO; 

outportbftty_base+4,RTS 
ou^rtbttty  base+1,0); 

ou^rtb(tty“base+3,DLAB  I  w_len  I  n_stop  ,  ^ 

ou^rtb(ttyn)ase,baud_table[baud][2]);  outportb(tty_'base+l,baud_table[baud][l]); 
out^rtb(tty  base+3,w_len  I  n_stop  |  parity); 
inportb(tty_5ase);  /*  Clear  any  pending  8250  receive  */ 
inportbttty  base+5);  /*  or  status  interrupts  */ 

inportb(tty]^ase+6);  /*  or  modem  interrupts  */ 

outportb(tty  base+l,0x07);  /*  Toggle  enable  bits  to  activate  */  ou^ortb(tty_base+l,0); 
ou^rtb(0x21,~irqlevel  &  inportbrox21));  /*  Enable  8259  interrupts  *! 
outportb(tty_base+4,OUT2  |  RTS  1  DTR);  I*  OUT2  is  wired  as  int  enable  *! 
ou^rtb(tty_base+ 1,0x07);  /*  Enable  recv,  xmit  &  status  interrupts  */  enable(); 


tty_close(port) 
int  port; 

/♦ 

Disables  all  interrupts  from  serial  port  previously  initialized  (tty_open).  Restores  original  interrupt 

service  routine  for  serial  port. 

ttvopen  must  have  been  previously  called. 

If  program  exits  without  calling  this  routine,  the  computer  will  likely 
crash. 

*/ 

{  . 

int  tty_base; 


if  (port  =  =  1) 

tty_base  =  ttyl_base; 
else 
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tty_base  =  tty2_base; 
disableO; 

outportb(0x21,irq_ level  1  inportb(0x21)); 

ou^rtbltty  base +4, RTS  |  DTR); 

ou^rtb(tty3)ase  +1,0); 

enaoleO; 

tty_flush(port); 

setvect(vector,old_isr) ; 

j  9|e  a|e  sK  )|e  aK  9K  9K  9)(  sR  * ’K  3K 

* 

*  Support  routines  for  selective  port  handling 

♦ 

int  tty_cnt(port) 
int  port; 

{ 

if  (port  =  =  1)  /*  return  char  from  i 

retum(tty2_cnt); 

int  tty_qfull(port) 
int  port; 

if  (port  =  =  1)  /*  return  char  from  ; 

retum(tty2_qfull) ; 

int  tty_error(port) 
int  port; 

{ 

if  (port  =  =  1)  /*  return  char  from  ; 

retum(tty2_error) ; 

int  tty_in(port) 
int  port; 

if  (port  =  =  1)  /*  return  char  from  ; 

retum(tty2_in()); 


/*  return  char  from  specified  port  */  retum(ttyl_cnt); 


/*  return  char  from  specified  port  */  retum(ttyl_qfull); 


/*  return  char  from  specified  port  */  retum(ttyl_error); 


/*  return  char  from  specified  port  */  retum(ttyl_in()); 


int  tty_out(port,byt) 
int  portjbyt; 

if  (port  =  =  1) 

return  (tty  l_out(by  t)) ; 
retum(tty2_out(byt)) ; 

int  tty_outs(port,str) 
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int  port; 
char  *str; 

{ 

if  (port  =  =  1) 

retum(tty  l_outs(str)) ; 
retum(tty2_outs(str)) ; 

int  tty_outmem(port,str,cnt) 
int  port; 
int  ^str; 
int  cnt; 

{ 

if  (port  =  =  1) 

retum(tty  l_outmem(str  ,cnt)) ; 
retum(tty2_outmem(str,cnt)) ; 

int  tty_flush(port) 
int  port; 

{ 

if  (port  =  =  1) 

retum(ttyl  flush()); 

^  retum(tty2_flush()); 


^j|cjJcj!«5!c***5i<3KiJ<9f«****3icH«*****si<5K*********************************************  ******/ 

^ ^ sic 9ie 9|e :|c 9ie sie sji: ^ ^ sic ^  ^  y  J  3.  t  C  ll  *******’^*************’^’^****/ 

/siCj|CSiCSK******5l«*****JiC5iC5iCSfC5K*****ij!nic***'5R************************************  * SjC * SjC * Sjc/ 

tty_in_niatch(char  *pattem,int  timeout  seconds) 

I* 

Tries  to  match  incoming  tty  character  stream  to  a  pattern  string. 

When  a  mismatch  is  found  the  matching  process  is  restarted. 

When  a  complete  match  is  found,  return  value  is  true. 

If  timeout  expires  before  a  match  is  found,  return  value  is  false. 

*1 

{  .  U  • 

mt  chj  i; 
long  timeout; 


} 


timeout  =  biostime(0,0)  +  18  *  timeout  seconds; 
i  =  0; 

while  (pattem[i]  !  =  0) 

^  if  (tty_error  1 1  tty_qfull  1 1  biostime(0,0)  >  timeout) 
retum(0); 

if  ((ch  =  tty  inO)  !  =  EOF  &&  ch  !  =  (pattem[i+  +]  &  OxFF)) 

} 

retum(l); 
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name  ttyisr 

;  large  memory  model 

;  **********  tjjig  version  has  no  Xon  Xoff  stuff  todd 

mosher 

;  TTYISR.ASM 
;  Task  25,  Robotics 
;  KL  Johnston,  SRL 

;  Turbo  C  functions  used  by  TTY68K,  Interrupt  service  £md  input/output 

?ueue  interface  routines  for  serial  (COM)  port  communications. 

Ised  adong  with  functions  in  TTYOPEN.C. 


EOF  =  -1 

EOI  =  20h 

INOSIZE  =  200h  ;  was  256 

NO^ERV  =  Olh  , 

OUTOSIZE  =  2020h  ;Que  size  limits  max  array  tty_outs  &  tty_outmem  can  handle. 

REC^ERV  =  04h 

XMIT"SERV  =  02h 

XOFF^  =  13h 

XON  =  llh 

public  tty  1  base 
public  _ttyl_error 
public  _ttyl_flush 
public  _ttyl_in 
public  _ttyl_isr 
public  _ttyl_out 
public  _ttyl_outmem 
public  ttylouts 
public  _ttyl_qfull 
public  ttylcnt 

public  _tty2_base 
public  _tty2_error 
public  _tty2_flush 
public  _tty2_in 
public  _tty2_isr 
public  _tty2_out 
public  _tty2_outmem 
public  _tty2_outs 
public  _tty2_qfull 
public  _tty2_cnt 

^data  segment  word  public  'data' 
in  1  head  dw  0 
ini  tail  dw  0 
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outlhead  dw  0 

outl  tail  dw  0 

xmitl_idle  dw  0 

sendl_xoff  dw  0 

sendl_xon  dw  0 

recvlpause  dw  0 

_ttyl  JBase  dw  0 

_ttyl_error  dw  0 

_ttyl_qfull  dw  0 

ttylcnt  dw  0 

in2_head  dw  0 

in2  tail  dw  0 
out2_head  dw  0 

out2  tail  dw  0 

xmit2  idle  dw  0 

send2_xoff  dw  0 

send2_xon  dw  0 

recv2pause  dw  0 

_tty2  JBase  dw  0 

tty2  error  dw  0 

_tty2_qfull  dw  0 

_tty2_cnt  dw  0 

_data  ends 

bss  segment  word  public  'bss' 
mql  db  INOSIZE  dup  (?) 
outql  db  OUTQSIZE  dup  (?) 
inq2  db  INQ^E  dup  (?) 
outq2  db  OUTt^SIZE  dup  (?) 

_bss  ends 

text  segment  byte  public  'code' 
dgroup  group  _data,_bss 

assume  cs?Jtext,ds:dgroup,ss:dgroup 

_ttyl_isr  proc  near 

;  Serial  port  interrupt  service  routine.  Never  directly  called  from  program. 

push  ax  ;Save  a  few  registers  to  work  with, 

push  bx 
push  dx 
push  ds 

mov  ax,dgroup  ;Set  up  ds  to  access  Turbo  C  objects, 
mov  ds,ax 

$20: 

mov  dx,_ttyl_base  ;Read  interrupt  identification 
register. 

add  dx,2 
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in  al,dx 

cmp  al.NO  SERV  ;Any  service  required? 
jne  $30  ;Yes, 

imp  done  ;No. 

$30: 

cmp  al,RECV_SERV  ;Service  received  data  interrupt? 

jne  notl_recv  ;No. 
mov  dx,_ttyl_base  ;Yes,  read  character, 

in  al,dx 

;  or  al,al  ;Null  character? 

;  jz  $20  ;Yes,  ignore  it. 

;  cmp  al,XOFF  ;  No,  Xoff  character? 

;  je  $20  ;Yes,  ignore  it. 

;  cmp  al,XON  ;No,  Xon  character? 

;  je  $20  ;Yes,  ignore  it. 

mov  bx,_ttyl_cnt  ;  increment  the 

inc  bx  ;  byte  count 

mov  ttyl  cntjbx  ;  and  save  it 
mov  5x,inl_head  ;Advance  input  que  head  pointer, 
inc  bx 

cmp  bx,INQ_  SIZE 

jl  $100 
xor  bx,bx 

$100; 

cmp  bx,inl_tail  ;Input  que  full? 

je  $200  ;Yes. 

mov  ini  head,bx  ;No,  save  new  que  head  pointer, 
mov  inqT[bx],al  ;Put  received  character  on  que. 
cmp  recvl_pause,0  ;Receiver  pause  already  flagged? 
jne  $20  ;Yes,  don't  bother  checking  ag<un. 

mov  ax.inl_tail  ;Calculate  free  space  remaining, 

sub  ax, ox 

dec  ax 
jge  $140 
add  ax,INQ_SIZE 

$140: 

;  cmp  ax,INQ_SIZE/4  ; Input  que  75%  full? 

;  jge  $20  ;No. 

;  mov  recvl_pause,l  ; Yes,  initiate  receiver  pause. 

;  mov  sendlxoff,! 

;  call  tickle  l_xmit 

jmp  $20 

$200: 

inc  twl_qfull  ;  Count  que  full  error, 
jmp  “$20  ;Check  for  more  servicing. 

notl_recv: 

cmp  al,XMIT_SERV  ;Service  transmit  ready  interrupt? 

jne  notl  xmit  ;No. 

cmp  sendl_xoff,0  ;Yes,  send  Xoff? 

je  $250  ;No. 

mov  sendl_xoff,0  ;Yes,  reset  flag. 
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mov  al,XOFF 
imp  $420 

$250: 

cmp  sendl  xon,0  ;Send  Xon? 

je  $260  "  :No. 

mov  sendl  xon,0  ;Yes,  reset  flag. 

mov  al,XON 

imp  $420 

$260: 

cmp  recvl_pause,0  ;Receiver  ISR  paused? 

jne  $290  ;Yes,  transmission  must  also  pause, 

mov  bXjOutl  tail  ;No,  output  que  empty? 
cmp  bx,outinb®3^d 

jne  $300  ;No. 

$290: 

mov  xmitl_idle,  1  ; Yes,  flag  outport  idle, 
imp  $20  ; Check  for  more  servicing. 

$300: 

inc  bx  ;Advance  que  tail  pointer, 

cmp  bx,OUTQ_ SIZE 

jl  $400 

xor  bx,bx 

$400: 

mov  outl_tail,bx  ;Save  new  que  tail  pointer, 

mov  al,outql[bx]  ;Get  character  to  be  transmitted. 

$420: 

mov  dx,  ttyl_base 

out  dx.m  ;Transmit  character, 

jmp  $20  ; Check  for  more  servicing. 

notl_xmit: 

mov  dx,  ttyl  base  ;Assume  status  interrupt, 
add  dx,5“ 

in  al,dx  ;Access  line  status  reg  to  clear 

error. 

inc  dx 

in  al,dx  ;Access  modem  status  reg  to  clear 

error. 

inc  tWl_error  ;Count  hardware  error, 

jmp  ”$20  ;Check  for  more  servicing. 

done: 

mov  al,EOI  ;Non-specific  EOI  for  8259. 

out  20h,al 

pop  ds  ;Restore  registers. 

pop  dx 

pop  bx 

pop  ax 

iret 

ttyljsr  endp 
ttyljn  proc  far 
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;  Returns  next  character  on  input  que  to  caller  as  an  integer. 

;  If  input  que  is  empty,  returns  EOF  (-1). 

mov  ax, EOF  ;Assume  que  empty, 

mov  bx,inl  tail 

cmp  bx,ininhead  ;Input  que  empty? 

je  $1200  ;Yes. 

cli 

mov  bx,_ttyl_cnt  ;  decrement  the 
dec  bx  ;  byte  count 

mov  _ttyl_cnt,bx  ;  and  save  it 
sti 

mov  bx,inl_tail 

inc  bx  ;No,  advance  input  que  tail  pointer, 

cmp  bx,INO  SIZE 
jl  $1100 
xor  bx,bx 
$1100: 

mov  al,inql[bx]  ;Retum  next  character  from  input 

que. 

xor  ah,ah 

mov  ini  tail,bx  ;Save  new  tail  pointer. 

$1200: 

push  ax  ;Save  return  value, 

cmp  recvl  pause,0  ;Receiver  paused? 
je  $1900  ;No. 

mov  ax,inl_head  ;Yes,  calculate  characters  left  in 
que. 

sub  ax.bx 
jge  $1j00 
add  ax,INQ_SIZE 
$1300: 

cmp  ax,INQ_SIZE/4  ; Input  que  75%  free? 
jge  $1900  ;No. 

cli  ;Yes,  no  interrupts  while  changing 

pause. 

mov  recvl_pause,0  ;Clear  receiver  ISR  pause, 
mov  sendlxon,! 
call  ticklel_xmit 
sti 

$1900: 

pop  ax 
ret 

ttylin  endp 

ttylout  proc  far 

;  Adds  a  character  (supplied  by  caller  as  an  integer)  to  ouq)ut  que. 
;  If  output  que  is  full,  cnaracter  is  not  queued  and  function 
;  returns  false  (0),  else  function  returns  true  (1). 

push  bp  ;Set  up  Turbo  C  call  frame. 
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mov  bp,M) 

;  cmp  byte  ptr  [bp+6],XOFF  ;Caller  sending  Xoff? 

;  je  $2800  ;Yes,  ignore  it. 

mov  bx,outl_head  ;No,  advance  output  que  head  pointer, 
inc  bx 

cmp  bx,OUTQ_ SIZE 

jl  $2300 
xor  bxjbx 
$2300: 

xor  ax, ax  ;Assume  failure. 

cmp  bx.outl  tail  ;Output  que  full? 

je  $2900  ;Yes,  return  failure. 

mov  al,[bp+6]  ;No,  get  character  passed  by  caller. 

mov  outqI[bx].al  ; Add  character  to  output  que. 

mov  out!  heaa,bx  ;Save  new  head  pointer. 

cli 

call  ticklel_xmit 
sti 

$2800: 

mov  ax,l  ;Retum  success. 

$2900: 

pop  bp 
ret 

_ttyl_out  endp 

_ttyl_outs  proc  far 

;  Adds  an  entire  null  terminated  string  (supplied  by  caller)  to  ouq)ut  que. 
;  If  string  is  longer  than  outout  que,  imse  (0)  is  return^. 

;  If  there  is  currently  insufficient  free  space  for  the  entire  string  to 
;  be  placed  on  the  output  que,  false  (0)  is  returned. 

;  If  string  length  is  zero,  true  (1)  is  returned,  but  no  action  takes  place. 

;  Else  stnng  is  placed  on  output  que  and  true  (1)  is  returned. 

push  bp  ;Set  up  Turbo  C  call  frame, 

mov  bp,sp 

push  si  ;Save  callers  index  registers, 

push  di 
push  ds 

pop  es  ;Pointer  ES:DI  to  callers  string, 

mov  di,[bp+6] 

mov  cx,OUTQ_SIZE+2  ;Max  legal  string  length  including 
null  +  1. 

xor  ax, ax  ;0  =  end  of  string, 

cld  ;Make  sure  we  increment  DI. 

repnz  scasb  ;String  too  long? 

jcxz  $3800  ;Yes. 

sub  cx,OUTQ_SIZE+l  ;No,  calculate  length  of  string. 
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neg  cx 

je  $3700  ;Zero  length  string,  all  done. 

$3000: 

mov  ax,outl_tail  ;Calculate  free  space  assuming  tail  > 

head. 

sub  ax,outl_head 
dec  ax 
jge  $3100 

add  ax,OUTQ_SIZE  ;Tail  <  =  head,  adjust  for  que  wrap 
around.  $3100: 

cmp  ax,cx  ;Enough  free  space  for  entire  string? 

jl  $3800  ;No. 

mov  si,[bp+6]  ;Yes,  set  up  to  xfer  string  to  que. 
lea  dijOutql 
add  di, out!  head 

inc  di 
$3200: 

cmp  di,offset  dgroup;outql  +OlJTQ_SIZE  ;Need  to  wrap  pointer 
around? 

il  $3300  ;No. 

lea  dijOutql  ;Yes. 

$3300: 

movsb  ;Transfer  one  byte  to  output  que. 

loop  $3200 
dec  di 

sub  di,offset  dgroup:outql  ;Calculate  new  que  head 
pointer. 

mov  outl_head,di  ;Save  new  que  head, 
cli 

call  ticklel_xmit 
sti 

$3700: 

mov  ax.l  ; Return  success, 

jmp  $3900 
$3800: 

xor  ax, ax  ;Retum  failure. 

$3900: 

pop  di 
pop  si 
pop  bp 
ret 

;  Alternate  entry  for  tty  outs.  Queues  an  array  of  bytes  the  size  of 
which 

;  is  specified  by  the  caller. 

^ttyl_outmem: 

push  bp  ;Set  up  Turbo  C  call  frame, 

mov  bp,sp 

push  si  ;Save  callers  index  registers, 

push  di 
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push  ds 

pop  es  ;Initialize  ES  =  DS. 

mov  cx,[bp+6]  ;Get  callers  byte  count, 
or  cx,cx  ;Byte  count  less  than  or  equal  zero? 

jl  $3800  ;Yes,  illegal, 

je  $3700  ;Yes,  all  done, 

cmp  cx.OUTQ  SIZE  ;No,  byte  count  too  high? 
ig  $386o  ■  ;Yes. 

jmp  $3000  ;No. 

ttylouts  endp 


ttyl_flush  proc  far 

Reset  all  input  and  output  queue  pointers  (losing  any  characters 
currently  in  either  queue).  Also  resets  error  counters. 

No  arguments  or  return  values. 


xor 

cli 

mov 

mov 

mov 

mov 

mov 

mov 

mov 

mov 

mov 

mov 

mov 

sti 

ret 

ttyl_flush 


ax, ax 

inl_head,ax 
ini  tail, ax 
outT_head,ax 
outl_tail,ax 
_ttyl  cnt,ax 
xmifT_idle,  1 
_ttyl_error,ax 
_ttyl  qfull,ax 
sendI^xoff,ax 
sendl_xon,ax 
recvl_pause,ax 


endp 


;Reset  input  que. 

;Reset  output  que. 

;  reset  byte  count 
;Reset  error  counts. 

;  Reset  Xon/Xoff  flags. 


ticldel_xmit  proc  near 

cmp  xmitl  idle,0  ;Transmitter  ISR  idle? 
je  $5800  “  ;No. 

mov  dx,_ttyl_base  ;Yes,  toggle  xmit  interrupt  enable  to 
inc  dx  ;  force  xmit  interrupt, 

in  al.dx 
mov  Dl.al 
and  al,0FDh 
out  dx.al 
mov  al,bl 
out  dx,al 
mov  xmitl  idle,0 
$5800: 
ret 

ticklel_xmit  endp 
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.««««*««****«***«  routines  for  port  2  *************** 

_tty2_isr  proc  near 

;  Serial  port  interrupt  service  routine.  Never  directly  called  from  program. 

push  ax  ;Save  a  few  registers  to  work  with, 

push  bx 
push  dx 
push  ds 

mov  aXjdgroup  ;Set  up  ds  to  access  Turbo  C  objects, 
mov  ds,ax 

$22: 

mov  dx,_tty2_base  ;Read  interrupt  identification 

register. 

add  dx.2 
in  al,ax 

cmp  al,NO_SERV  ;Any  service  required? 

jne  $32  ;Yes. 

imp  done2  ;No. 

$32: 

cmp  al,RECV_SERV  ;Service  received  data  interrupt? 
jne  not2_recv  ;No. 

mov  dx,_tty2_base  ;Yes,  read  character, 
in  al,dx 

;  or  al,al  ;Null  character? 

;  jz  $22  ;Yes,  ignore  it. 

;  cmp  aljXOFF  ;No,  Xoff  character? 

;  je  $22  ;Yes,  ignore  it. 

;  cmp  al,XON  ;No,  Xon  character? 

;  je  $22  ;  Yes,  ignore  it. 

mov  bx,_tty2_cnt  ;  increment  the 

inc  bx  ;  byte  count 

mov  tty2  cnt,bx  ;  and  save  it 

mov  5x,iri2_head  ;Advance  input  que  head  pointer, 
inc  bx 

cmp  bx,INQ  SIZE 
jl  $102 
xor  bx,bx 

$102: 

cmp  bx,in2_tail  ;Input  que  full? 
je  $202  ;Yes. 

mov  in2  head,bx  ;No,  save  new  que  head  pointer, 
mov  inq2[bx],al  ;Put  received  character  on  que. 
cmp  recv2_pause,0  ;Receiver  pause  already  flagged? 
jne  $22  ;Yes,  don't  bother  checking  again, 

mov  ax,in2_tail  ; Calculate  free  space  remaining, 

sub  ax,  ox 

dec  ax 
jge  $142 
add  ax,INQ  SIZE 

$142: 
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ax,INQ_SIZE/4  ; Input  que  75  %  full? 

$22  ;No. 

recv2_pause,  1  ;  Yes,  initiate  receiver  pause. 

send2_xoff,  1 
tickle2  xmit 
$22  “ 

tW2_qfull  ;Count  que  full  error. 

"$22  ; Check  for  more  servicing. 

al,XMIT_SERV  ;Service  transmit  ready  interrupt? 
not2  xmit  ;No. 
send2  xoff,0  ;Yes,  send  Xoff? 

$252  "  ;No. 

send2  xoff,0  ;Yes,  reset  flag. 
al,XO'PF 
$422 

send2  xon,0  ;Send  Xon? 

$262  ~  ;No. 

send2  xon,()  ;Yes,  reset  flag. 

al,XON 
$422 

recv2_pause,0  ;  Receiver  ISR  paused? 

$292  ;Yes,  transmission  must  also  pause. 

bx,out2  tail  ;No,  output  que  empty? 
bx,out2Tiead 
$302  “  ;No. 

xmit2_idle,l  ;Yes,  flag  outport  idle. 

$22  ;Check  for  more  servicing. 

bx  ;Advance  que  tail  pointer. 

bx,OUTO  SIZE 
$402 
bx,bx 

out2_taiI,bx  ;Save  new  que  tail  pointer. 
al,outq2^x]  ;Get  character  to  be  transmitted. 

dx,  tty2_base 

dx,m  ;Transmit  character. 

$22  ;Check  for  more  servicing. 

dx,  tty2  base  ;Assume  status  interrupt. 
dx.5~ 

al,ax  ;Access  line  status  reg  to  clear 

dx 

al,dx  ;  Access  modem  status  reg  to  clear  error. 
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inc  _tty2_error  ;Count  hardware  error. 

jmp  $22  ; Check  for  more  servicing. 

done2: 

mov  al,EOI  ;Non-specific  EOI  for  8259. 
out  20h,al 

pop  ds  ;Restore  registers. 

pop  dx 

pop  bx 

pop  ax 

iret 

_tty2_isr  endp 
_tty2_in  proc  far 

;  Returns  next  character  on  input  que  to  caller  as  an  integer. 

;  If  input  que  is  empty,  returns  EOF  (-1). 

mov  ax, EOF  ;Assume  que  empty, 

mov  bx,in2  tail 

cmp  bx,in2nhead  ;Input  que  empty? 

je  $1202  "  ;Yes. 

cli 

mov  bx,_tty2_cnt  ;  decrement  the 
dec  bx  ;  byte  count 

mov  _tty2_cnt,bx  ;  and  save  it 
sti 

mov  bx,in2_tail 

inc  bx  ;No,  advance  input  que  tail  pointer, 

cmp  bx,INQ_SIZE 
jl  $1102 
xor  bx,bx 
$1102: 

mov  al,inq2[bx]  ;Retum  next  character  from  input 

que. 

xor  ah,ah 

mov  in2  tail,bx  ;Save  new  tail  pointer. 

$1202: 

push  ax  ;Save  return  value, 

cmp  recv2  pause,0  ;Receiver  paused? 
je  $1902  ;No. 

mov  ax,in2_head  ;Yes,  calculate  characters  left  in 
que. 

sub  ax.bx 
jge  $1302 
add  ax,INQ_SIZE 
$1302: 

cmp  ax,INO  SIZE/4  ;Input  que  75  %  free? 
jge  $1902  ;No. 

cli  ;Yes,  no  interrupts  while  changing 

pause. 

mov  recv2_pause,0  ;  Clear  receiver  ISR  pause. 
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mov  send2_xon,l 
call  tickle2_xmit 
sti 

$1902: 

pop  ax 
ret 

_tty2_in  endp 

_tty2_out  proc  far 

;  Adds  a  character  (supplied  by  caller  as  an  integer)  to  ouQ)ut  que. 

;  If  output  que  is  full,  cnaracter  is  not  queued  and  function 
;  returns  false  (0),  else  function  returns  true  (1). 

push  bp  ;Set  up  Turbo  C  call  frame, 

mov  bp,sp 

;  cmp  byte  ptr  [bp+6],XOFF  ; Caller  sending  Xoff? 

;  je  $2802  ;Yes,  ignore  it. 

mov  bx,out2_head  ;No,  advance  output  que  head  pointer, 
inc  bx 

cmp  bXjOUTOSIZE 
jl  $2302 
xor  bxjbx 
$2302: 

xor  ax, ax  ;Assume  failure. 

cmp  bx,out2_tail  ;Output  que  full? 

je  $2902  ;Yes,  return  failure. 

mov  al,[bp+6]  ;No,  get  character  passed  by  caller. 

mov  outqznjxTal  ;Ad(rcharacter  to  output  que. 

mov  outz  nead,bx  ;Save  new  head  pointer. 

cli 

call  tickle2_xmit 
sti 

$2802: 

mov  ax,  1  ;Retum  success. 

$2902: 

pop  bp 
ret 

_tty2_out  endp 

_tty2_outs  proc  far 

;  Adds  an  entire  null  terminated  string  (supplied  by  caller)  to  output  que. 
;  If  string  is  longer  than  output  que,  imse  (0)  is  returned. 

;  If  there  is  currently  insufficient  free  space  for  the  entire  string  to 
;  be  placed  on  the  output  que,  false  (0)  is  returned. 

;  If  string  length  is  zero,  true  (1)  is  returned,  but  no  action  takes  place. 

;  Else  string  is  placed  on  output  que  and  true  (1)  is  returned. 
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$3Qp^: 


push  bp  ;Set  up  Turbo  C  call  frame, 

mov  bp,sp 

push  si  ;Save  callers  index  registers, 

push  di 
push  ds 

pop  es  ;Pointer  ES:DI  to  callers  string. 

mov  di,[bp+6]  . 

mov  cx,OUT(^SIZE+2  ;Max  legal  string  length  including 
null  +  1. 

xor  ax, ax  ;0  =  end  of  string, 

cld  ;Make  sure  we  increment  DI. 

repnz  scasb  ;  String  too  long? 

jcxz  $3802  ;Yes. 

sub  cx,OUTQ_SIZE+l  ;No,  calculate  length  of  string, 
neg  cx 

le  $3702  ;Zero  length  string,  all  done. 


head. 


mov  ax,out2_tail  ; Calculate  free  space  assuming  tail  > 


sub  ax,out2_head 
dec  ax 
jge  $3102 

add  ax,OUTQ_SIZE  ;Tail  <  =  head,  adjust  for  que  wrap 
around.  $3102: 

cmp  ax,cx  ;Enough  free  space  for  entire  string? 

jl  $3802  ;No. 

mov  si,  [bp +6]  ;Yes,  set  up  to  xfer  string  to  que. 
lea  dijOutq2 
add  di,out2_head 
inc  di 


$3202: 

cmp  di, offset  dgroup:outqH-OUTQ_SIZE  ;Need  to  wrap  pointer 

around? 

il  $3302  ;No. 

lea  di,outq2  ;Yes. 

$3302: 

movsb  ;Transfer  one  byte  to  output  que. 

loop  $3202 
dec  di 

sub  di,offset  dgroup:outq2  ;Calculate  new  que  head 
pointer. 

mov  out2  head,di  ;Save  new  que  head, 
cli 

call  ticlde2_xmit 
sti 

$3702: 

mov  ax,  1  ; Return  success, 

imp  $3902 
$3802^: 

xor  ax,ax  ;Retum  failure. 

$3902: 
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pop  di 
pop  si 
pop  bp 
ret 

;  Alternate  entry  for  _tW_outs.  Queues  an  array  of  bytes  the  size  of  which 
;  is  specified  by  the  caller. 

tty2  outmem; 

push  bp  ;Set  up  Turbo  C  call  frame, 

mov  bp,sp 

push  si  ;Save  callers  index  registers, 

push  di 
push  ds 

pop  es  ;Initialize  ES  =  DS. 

mov  cx,[bp+6]  ;Get  callers  byte  count. 

or  cx.cx  ;Byte  count  less  than  or  equal  zero? 

jl  $3802  ;Yes,  illegal. 

je  $3702  ;Yes,  all  done. 

cmp  cx,OUTQ_SIZE  ;No,  byte  count  too  high? 

jg  $3802  ;Yes. 

jmp  $3002  ;No. 

_tty2_outs  endp 

_tty2_flush  proc  far 

;  Reset  all  input  and  ouq)ut  queue  pointers  (losing  any  characters 
;  currently  in  either  queue).  Also  resets  error  counters. 

;  No  arguments  or  return  values. 

xor  ax, ax 

cli 

mov  in2_head,ax  ;Reset  input  que. 

mov  in2  tail, ax 

mov  out2_head,ax  ;Reset  output  que. 

mov  out2  tail,ax 

mov  _tty2‘  cnt,ax  ;reset  byte  count 

mov  xmit2^idle,  1 

mov  _tty2_error,ax  ;Reset  error  counts, 

mov  _tty2  qfull,ax 

mov  send2^xoff,ax  ;Reset  Xon/Xoff  flags. 

mov  send2_xon,ax 

mov  recv2_pause,ax 

sti 

ret 

_tty2_flush  endp 

tickle2_xmit  proc  near 

cmp  xmit2  idle,0  ;Transmitter  ISR  idle? 
je  $5802  "  ;No. 
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mov  dx,_tty2_base  ;Yes,  toggle  xmit  interrupt  enable  to 
inc  dx  ;  force  xmit  interrupt, 

in  al,dx 
mov  bl.al 
and  al,0FDh 
out  dx,al 
mov  al,bl 
out  dx,al 
mov  xmit2  idle,0 
$5802: 
ret 

tickle2_xmit  endp 
text  ends 

end 
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^  *  3|c  jje  }|e  **  :|c  **  :ie  5|c  *  :fc  *  5ic  *  *  :|c  3K  lie  ***********  ♦ 

* 

*  merlin.def  global  definition  file 

* 

*3|c3ic3ic:(e:Jc:|e5|c3(£**********************************/ 


#include  "merlin, typ" 

/*******  The  following  memory  assignments  simply  map  the  */ 

/*******  PC  hshi  control  structures  to  the  MERLIN  structures  */ 

/*******  The  assignments  were  copied  out  of  the  HSHI  reference  manual  */ 

/*******  pages  9  and  10.  When  used  by  the  code,  comments  will  */ 

/*******  explain  their  use.  For  more  info  read  the  HSHI  manual,  */ 

/*******  it  IS  easy  to  follow  and  it  is  short!  */ 

HC  BUF  huge  *hc  buf  =  (HC  BUF  huge  *)  (0x00  +  MEM_OFFSET); 

HR~BUF  huge  *hr'l)uf  =  (HRIBUF  huge  *)  (0x08  +  MEM  OFFSET); 

HR“RSP  huge  *hr  Jsp  =  (HR  TlSP  huge  *)  (OxOe  +  MEM  DFFSET); 

SRV  PAR  huge  *hc  srv  =  (SRV  PAR  huge  *)  (0x10  +  MEM  OFFSET); 

SPNT  huge  *hc  epos  =  (SPNTliuge  *)  (0x60  +  MEM  OFFSET); 

float  huge  *hc  cvef  =  (float  huge  ’•')  (0x80  +  MEM  OFFSET); 

JOINTS  huge  “^c Jpos  =  (JOINTS  huge  *)  (OxaO  +  MEM  OFFSET); 

JOINTS  huge  *hc  ivel  =  (JOINTS  huge  *)  (OxcO  +  MEM  OFFSET); 

UOINTS  huge  *hc  mpos  =  (UOINTS  huge  *)  (OxeO  +  MEM  OFFSET); 

LJOINTS  huge  *hc_mvel  =  (LJOINTS  huge  *)  (0x100  +  MEM^OFFSET); 

SPNT  huge  *hr  epos  =  (SPNT  huge  *)  (0x200  +  MEM  OFFSET); 

JOINTS  huge  *hfjpos  =  (JOINTS  huge  *)  (0x220  +  MEM^OFFSET); 

JOINTS  huge  *hrjvel  =  (JOINTS  huge  *)  (0x240  +  MEM  OFFSET); 

LJOINTS  huge  *hr  mpos  =  (LJOINTS  huge  *)  (0x260  +  MEM_OFFSET); 

UOINTS  huge  *hr“mvel  =  (UOINTS  huge  *)  (0x280  +  MEM  OFFSET); 

UOINTS  huge  *hr_mcyc  =  (UOINTS  huge  *)  (0x2a0  +  MEN^OFFSET); 

/**♦*♦♦♦**  conversions  and  limits  for  encoders  */ 

double  dtoe[6]  =  /*  conversion  for  degrees  to  encoder  ents  */ 

/*  ranges  are  +-  48000  for  j  1,2,3  */ 

/*  +- 24000  for  i  4,6  */ 

/*  +-  12000  for  j  5  */ 

I*  take  range  /  180  (90  for  j  5)  for  conv  */ 
{266.66667,266.66667,266.66667,133.333,133.333,133.333}; 

double  rtoe[6]  =  /*  conversion  for  rads  to  encoder  ents  */ 

/*  ranges  are  +-  48000  for  j  1,2,3  */ 

/*  +- 24000  fori  4,6  */ 

/*  +-  12000  for  J  5  */ 

/*  take  range  /  pi  (pi/2  for  j  5)  for  conv  */ 
{15278.8745,15278.8745,15270745,7639.437,7639.437,7639.437}; 
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long  encmin[6]  =  /*  min  encoder  reading  */ 

{-48000, -48000, -48000, -48000, -12000, -48000}; 
long  encmax[6]  =  /*  max  encoder  reading  */ 

{48000,48000,48000,48000, 12000,480(W} ; 
double  degminm]  =  /*  min  degrees  */ 

{^170,-170,-170,-360,-85,-3^}; 
double  degmax[6]  =  /*  max  degrees  */ 

{170, 170, 170,360,85,360} ; 
double  radmin[6]  =  /*  min  rads  */ 

{-2.967,-2.967,-2.967,-6.283,-1.4835,-6.283}; 
double  radmaxlb]  =  /*  max  rads  */ 

{2.967,2.967,2.967,6.283, 1 .4835,6.283} ; 


intmax  srv  acc[6]  =  {12,12,12,12,12,20};  /*  default  servo  accel  0..  16*/ 
intmax  srv  vel[6]  =  (8,8,8,8,8,12};  /*  default  servo  veloc  0.. 32*/ 

int  gainr6]  =  {4,4,4,4,4,6};  /*  default  servo  gain  1..8*/ 

double  x_pos;  /*  x  end  tip  position  */ 

double  y_pos;  /*  y  end  tip  position  */ 

double  z_pos;  /*  z  end  tip  ^sition  */ 

double  roll;  /*  gripper  roll  */ 

double  pitch;  /*  gnpper  pitch  */ 

double  yaw;  /*  gnpper  yaw  */ 

double  a[5]  =  {0,0,17.3,0,0}^;  /*  Denvait  Hartenberg  parameters  */ 

double  Ma[5]  =  (0,-1.5707.3.14159,-1.5707,-1.75071;  /*  "  -90,180,-90,-90 
double  d[5]  =  {0,0,-i2,0, 17.25};  /*  "  */ 

double  h[4];  /*  vars  used  by  inverse  kin  */ 

double  hl_partial; 
double  theta[6]  =  {0} ; 


*/ 


double  in  merlin  ioint[6]: 

double  gnpper_tip[4]  =  {0,0,-10.5,l};  /*  gripper  tip  x,y,z  coordinates  */ 


/*  defined  w/respect  to  tool  roll  */  /*  coordinate  system  */ 
double  Ct2,st2,nst2,ct3,nct3,st3,nst3,ct4,nct4,st4,nst4, 

Ct5,nct5,st5,nst5,ct6,nct6,st6,nst6,ct7,st7,nst7, 

ct8,nct8,st8,nst8; 

/*  nct7  is  not  used  */ 

/*  used  in  tmatQ  to  allow  computation  of  */ 
/*  cos(*tX)  &  sm(*tX)  only  once  */ 


double  stl  0[4][4]  =  {{  0, 0,0,0}, 

{  0, 0,0,0  }, 

( O.O.O.O  I, 
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double  st2  0[4][4]  =  {{  0,0,0,01, 

{ 0,0,0,0 }, 
{0,0,0,!}}; 

double  st3_0[4][4]  =  {{  0, 0,0,0^,^  ^  ^ 


0, 0,0,0  1, 

0,0,0,!}); 


double  st4_0[4][4]  =  {{  0,0,0, Oj, 


'i’o,o,o }, 
0,0,0,0 }, 
{ololo’M 


double  st5_0[4][4]  =  {{  0,0,0,0  j. 


1’ 

0, 0,0,0  1, 

{o:o:o:i}r; 


double  st6_0[4][4]  =  {{  0,0,0,01, 


double  srl_0[4][4]  = 


i’2’2’2  }’ 

{  0,0,0,0  i, 
{0,0,0,!}); 

double  sr2_0[4][4]  =  {{  0,0,0,0J,^  ^  ^ 

I  o;o;o;o  j; 

{  0,0,0,!}); 

double  sr3  0[4][4]  =  {{  0,0,0,0  }, 

{  0,0,0,0  1, 
{0,0,0,!); 


double  wrist_roll;  /*  slave  wrist  roll  */ 

double  wrist_flex;  /*  slave  wrist  flex  */ 

double  tool_roll;  /*  slave  tool  roll  */ 

/*  values  for  converting  optical  encoder  vals  */ 

/*  to  radians  */ 

double  oe  to  rad[8]  =  {  ENC  RAD,  ENC  RAD,  UA  ENC  RAD,  ENC  RAD, 

"  LA_ENC_RAD,  ENC_RAD,  ENC_RAD,  0  }; 
/♦double  hs  1ft  deg[8]  =  /*  fixed,  measured  hardstop  angles  */ 

/♦  “{  a,o,o,o,o,o,o,o};  ♦/ 
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int  arm_hs_oe[8];  /*  hard  stop  values  of  the  optical  encoder  */ 

double  exo  1  hs  rad[8]  =  ^  ^ , 

/*  exo  left  arm  joint  encoder  clockwise  hardstop  (rads) 
{1.57,  /*  shoulder  azimuth  */ 

0.8098,  /*  shoulder  elevation  */ 

-0.7662,  /*  upper  arm  roll  */ 

0.785,  /*  elbow  flex  */ 

-2.12,  /*  lower  arm  roll  */ 

1.284,  /*  wrist  radial  */ 

2.543,  /*  wrist  flex  */ 

0.0  /*  gripper  */ 


double  exo  r  hs  rad[8]  = 

/*  exo  right  arm  joint  encoder  clockwise  hardstop  (rads)  */ 
{  2.34,  /*  shoulder  azimuth  */ 

0.0349,  /*  shoulder  elevation  */ 

0.0  ,  /*  upper  arm  roll  */ 

0.8028,  /*  elbow  flex  */ 

-1.6985,  /*  lower  arm  roll  */ 

1.284,  /*  wrist  radial  */ 

-0.82W,  /*  wrist  flex  */ 

0.0  /*  gripper  */ 

}; 


double  11  =  6.9375 
double  12  =  5.3075 
double  13  =  5.5156 
double  14  =  4.9375 
double  15  =  12.141 
double  16  =  11.250 


double  exo_r_arm[8]; 
double  exo_l_arm[8]; 

double  mt[4][4]  =  {{  0, 0,0,0  }, 

{ 0,0,0,0 } 
{  0,0,0,0  1 
(0,0,0,  111 

double  mt2  0[4][4]  =  {{  0,0,0,0  }, 

i  0,0,0,0 } 
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{{  0,0,0  0}  „  „  , 

9'9'9>9 1> 
o,o,o,o  1, 

{{  0,0, 0.0  ]. 
’{o,Uo,o}, 
{  0, 0,0,0  1, 
{0  0,0,1}); 
{{ 0,0,0  0  ) 

0,0,0,0i, 
0.0,0,!}); 
{{ 0,0,0  0  } 

{  0,0,0,0  }, 
0,0,0, 1}); 
{{  0,0,0,0  }. 
’{o,Uo,o}, 
{  0,0,0,0  }, 
{0:0,0,1}); 


double  mr6  5[4][4]  =  {{  0,0,0.0  }, 

{0,0,0,0 


double  mt3_0[4][4] 
double  mt4_0[4][4] 
double  mt5_0[4][4] 
double  mt6_0[4][4] 
double  mt7_0[4][4] 
double  mt8_0[4][4] 


i0,0,0,0), 
0, 0,0,0  }, 
:0A0,1}); 


double  mr7  5[4][4]  =  {{  0,0,0;,0),’ 

0,0, 0,0  1, 

l  0,0,0,!}); 

double  mrS  5[4][4]  =  {{  0,0,0.0  }, 

(0,0,0,01, 

0,0,0,01, 

{0,0,0,!}); 

double  mr9  5[4](4]  =  {{  0,0,0.0  }, 

(0,0,0,01, 

0,0,0,01, 

{0,0,0,!}); 

^  3|e  9|c  3|e  ^  9|e  9|e 3ic  3ie  ^  aioK  sK 


♦ 

♦ 


indexing  vars 


)|c)ic)k  He  9k  3k  9|c  9|e  9(C  9|e  9K  3te  9|e  9k ))« 9K  He  9tc  9|cy 

double  x_offset;  /*  indexing  offsets  */ 

double  y_offset;  /*  indexing  offsets  */ 

double  z^offset;  /*  indexing  offsets  */ 

int  indexing  =  0; 

double  initx_offset  =  5;  /*  initial  offsets  for  work  space  matching  */ 
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double  inity_offset  =  0; 
double  initz_offset  = 
int  lock_wnst  =  0; 
int  mba_rqst  =  'H'; 
int  done  =  0; 
int  badcnt  =  0; 


*  is  wrist  locked  in  cal  position  */ 

/*  requesting  Left  Right  or  H/Both  */ 
gen  purpose  done  flag  */ 

'  count  of  bad  comm  messages  */ 
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y  ♦  5|e  3|c  *  He  5|c  ♦***♦*****  3K  JK  ♦♦***  * 

♦ 

*  merlin. def  global  definition  file 

♦ 

He  He  He  ♦  He  ♦♦  He  He  He  He  He  He  He  He  He  He  He  ♦  He  He  He  He  He  He  *  He  He  He  He  He  He  He  H<  He  He  He  He  He  He  He  *  He  y 


jWnclude  "merlin.typ" 

/♦*****♦  The  following  memory  assignments  simply  map  the  *! 

I*******  PC  hshi  control  structures  to  the  MERLIN  structures  */ 

/♦♦**♦♦*  The  assignments  were  copied  out  of  the  HSHI  reference  manual  */ 
/*******  pages  9  and  10.  When  used  by  the  code,  comments  will  */ 

/*******  explain  their  use.  For  more  info  read  the  HSHI  manual,  */ 

/*******  it  IS  easy  to  follow  and  it  is  short!  */ 

extern  HC  BUF  huge  *hc  buf; 
extern  HR~BUF  huge  *hrl)uf; 
extern  HR~RSP  huge  *hr_rsp; 

extern  SRV  PAR  huge  *hc_srv; 
extern  SPNT  huge  *hc_cpos; 

extern  float  huge  ♦he  cvel; 
extern  JOINTS  nuge  ’•^cjpos; 

extern  JOINTS  huge  *hcjvel; 

extern  UOINTS  huge  *hc_mpos; 
extern  UOINTS  huge  *hc_mvel; 

extern  SPNT  huge  *hr_cpos; 

extern  JOINTS  huge  *hrjpos; 

extern  JOINTS  huge  ♦hrjvel; 

extern  UOINTS  huge  *hr_mpos; 
extern  UOINTS  huge  *hr_mvel; 
extern  UOINTS  huge  *hr_mcyc; 

/*********  conversions  and  limits  for  encoders  */ 

extern  double  dtoe[6];  /*  conversion  for  degrees  to  encoder  ents  */ 

/*  ranges  are  +-  48000  for  j  1,2,3  */ 

/*  +- 24000  fori  4,6  ♦/ 

/*  +-  12000  for  j  5  */ 

/♦  take  range  /  180  (90  for  j  5)  for  conv  */ 
extern  double  rtoe[6];  /*  conversion  for  rads  to  encoder  ents  */ 

I*  ranges  are  +-  48()()0  for  j  1,2,3  */ 

/*  +- 24000  fori  4,6  */ 

/*  +-  12000  for  J  5  */ 

/*  take  range  /  pi  (pi/2  for  j  5)  for  conv  */ 
extern  long  encminC^;  /*  min  encoder  reading  */ 
extern  long  encmax[o];  f*  max  encoder  reading  */ 
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extern 

extern 

extern 

extern 

extern 

extern 

extern 

extern 

extern 

extern 

extern 

extern 

extern 

extern 

extern 

extern 

extern 

extern 

extern 


double  degniin[^; 
double  degmax[6]; 
double  raamin[m; 
double  radmaxio]: 
int  max_srv_acc[o]; 
int  max  srv_vel[6]; 
int  eainfb]; 
douole  x_pos; 
double  y_pos; 
double  zpos; 
double  roll; 
double  pitch; 
double  yaw; 


)h{ 


double  a 
double  alp 
double  dp 
double  h[4 
double  hl_^ 
double  theta[6]; 


/*  min  degrees  */ 

/*  max  degrees  */ 

I*  min  rads  */ 

/*  max  rads  */ 

/*  default  servo  accel  0..16*/ 
/*  default  servo  veloc  0.. 32*/ 
7*  default  servo  gain  1 . .  8*/ 

/*  X  end  tip  position  */ 
/*  y  end  tip  position  */ 
/*  z  end  tip  wsition  */ 
/*  gripper  roll  */ 

/*  gnpper  pitch  */ 

/*  gnpper  yaw  */ 


/*  Denvait  Hartenberg  parameters  */ 
I'aP];  /*  "  -90,180,-90,-90  */ 

/*  "  */ 

/*  vars  used  by  inverse  kin 

partial; 


*/ 


extern  double  in  merlinJoint[6]; 

extern  double  gffpper_tip[4];  /*  gripper  tip  x,y,z  coordinates  */ 


/*  defined  w/respect  to  tool  roll  */  /*  coordinate  system  */ 
extern  double  Ct2,st2,nst2,ct3,nct3,st3,nst3,ct4,nct4,st4,nst4, 
Ct5,nct5,st5,nst5,ct6,nct6,st6,nst6,ct7,st7,nst7, 

Ct8,nct8,st8,nst8; 

/*  nct7  is  not  used  */ 

/*  used  in  tmatO  to  allow  computation  of  */ 
/*  cos(*tX)  &  sin(*tX)  only  once  */ 


extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 
extern  double 


St  10 
st2  Oj 
st3  oj 
st4  0| 
st5“0 
st6"0[ 
srro 
sr2_0 
sr3  0 
wrist_r6ri; 
wristflex; 
toolroll; 


/*  slave  wrist  roll  */ 

/*  slave  wrist  flex  */ 

/*  slave  tool  roll  */ 

/*  values  for  converting  optical  encoder  vals  */ 
/♦to radians  */ 


extern  double  oe  to_rad[8]; 

/*  extern  double'hs_arm_deg[8];  */ 
extern  int  arm  hs_oe[8]; 

/*  clockwise  hardstop  values  in  radians  */ 
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extern  double  exo_r_hs_rad[8]; 

/*  most  cw  rotation  of  encoder  */ 

extern  double  exo_l_hs_rad[8]; 

/*  most  cw  rotation  of  encoder  */ 

extern  double  11; 
extern  double  12; 
extern  double  13; 
extern  double  14; 
extern  double  15; 
extern  double  16; 

extern  double  exo_r_arm[8]; 
extern  double  exo  1  arm[8]; 
extern  double  rntplp]; 
extern  double  mt2_0[4][4]; 
extern  double  mt3_0[4][4]; 
extern  double  mt4_0[4][4j; 
extern  double  mt5_0[4][4]; 
extern  double  mt6  0[4][4]; 
extern  double  mt7^0[4][4]; 
extern  double  mt8  0[4][4]; 
extern  double  mr6”5[4][4]; 
extern  double  mr7^5[4][4]; 
extern  double  mr8_5[4][4]; 
extern  double  mi9^  4;  [4]; 

* 

*  indexing  vars 

* 

extern  double  x_offset;  /*  indexing  offsets  */ 

extern  double  y_offset;  /*  indexing  offsets  */ 

extern  double  z  offset;  I*  indexing  offsets  */ 

extern  int  indexing; 

extern  double  initx_offset;  /*  initial  offsets  for  work  space  matching  */ 
extern  double  inity  offset; 
extern  double  initz  offset; 

extern  int  lock_wiist;  /*  is  wrist  locked  in  cal  position  */ 
extern  int  mba  rqst;  /*  requesting  Left  Right  or  H/Both  *! 
extern  int  done;  /*  gen  purpose  done  nag  */ 

extern  int  badcnt;  /*  count  of  oad  comm  messages  */ 
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♦ 

♦ 

♦ 

3|Cj|C3|CJie>|C***********************************/ 


This  is  the  type  declaration  file  for  Merlin 


/*  This  file  declares  the  data  types  needed  for  shared  memory  */ 

/*  HSHI  operations.  This  section  is  similar  to  that  given  *! 

/*  in  the  HSHI  Reference  Manual,  pp.  9  -  10.  */ 

^define  MBA  PORT  2 
#define  JR3_FORT  1 

#define  FFl  59 
#define  FF2  60 
#define  FF3  61 
jj^define  FF4  62 
#define  FF5  63 
^define  FF6  64 
#defineLA  75 
#defineRA  77 
#defineUA  72 
#defineDA  80 
^define  FHOME  71 
#define  FEND  79 
#define  PI  3.14159 
#define  DTORAD  .017453 
typedef  struct 

intcyc  elk; 
int  cmJ_no; 

char  command;  I*  Switched  from  HSHI  manual  */ 
char  buf_stat;  /*  See  page  9  */ 
int  eye  no; 

}  HC_BUF  ; 

#define  NUM_AXES  6 

^define  BUF  HOST  0x55 

^define  BUF“HSHI  Oxaa 
^define  sqr(x7  (x)  *  (x) 

#define  CMD  SET  PARAMS  0x01 
#define  CMD"EXIT  0x12 

#define  CMD  RD  M  STAT  0x03 
#define  CMD"M  VOS  0x08 

#define  CMD"M“VEL  0x09 

jl'define  CMD"RD  J  STAT  0x07 
^define  CMD"J  PDS  0x05 

^define  CMDTVEL  0x06 

#define  CMD"RD  C  STAT  0x04 
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^define  CMD  C  POS  0x02 
^define  CMD'SET  C  VEL  OxOc 
^define  CMD"J  INTERP  OxOd 
/^'define  CMD“(T  INTERP  OxOe 

^define  MEM_OFFSET  OxOdOOOOOOO 


typedef  struct 

int  ech_cnid; 
intech  eye; 
int  end” eye; 
}  HR_BUF  ; 


typedef  struct 


intrsp  bits; 
}  HR_RSP  ; 


typedef  struct 


long  max_acc; 
long  max  vel; 
long  gain; 

}  SRVPAR  ; 


typedef  struct 

^RYPAR  servo_param[6]  ; 
}  SRV_PAR ; 

typedef  struct 

float  x; 
float  y; 
float  z; 
float  roll; 
float  pitch; 
float  yaw; 

}  SPNT; 


typedef  struct 

float  axis[6]; 
}  JOINTS  ; 


typedef  struct 


merlin.typ 

#define  ENC  RAD  .001534  /*  conversion  encoder  cnts  to  radians  */ 

I*  Encoder  to  Joint  Gear  Ratio  1:1 

360  d^  joint  rev/  4096  encoder  cnts  rev 
=  .08'^  deg  per  encoder  cnt 
=  .001534  rad  per  encoder  cnt  */ 

#define  LA  ENC  RAD  .0017858  /*  conversion  lower  arm  roll  encoder  to  rad  */  /*  Encoder  to  Joint 

Gear  Ratio  7.33:1 

360  d^  joint  rev/480  *  7.33  encoder  cnts 
=  .  10232  deg  per  encoder  cnt 
=  .0017858  rad  per  encoder  cnt  */ 

/*#define  UA  ENC  RAD  .0015098  /*  conversion  upper  arm  roll  encoder  to  rad  */ 

/*  Encoder  to  Joint  Gear  Ratio  8.67:1  */ 

/*  360  deg  joint  rev/480  *  8.67  encoder  cnts  */  /*  = 

.0865  deg  per  encoder  cnt  */ 

/*  =  .0015^098  rad  per  encoder  cnt  */ 

#defme  UA  ENC  RAD  .00126363  /*  480cnts/encrev  *  10.3642encrev/armrev  */  /* 

/360deg/armrev  =  cnts/deg  */ 

/*  then  convert  to  rad/cnts  */ 
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11.0  Appendix  C:  Bit3  PC-AT  Adaptor  Card  Jumper  Settings 
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PC-AT  ADAPTOR  CARD  JUMPERS 


ii/j 

S'\4  3  pc /at 

21  D«.e  '2_ 


THE  SYS  JUMPERS  (AT  CARD) 

Locate  the  SYS  jumper  block  at  location  F2  on  the  AT  board. 


SYS 

o  o  1  jumper  If  you  want  to  select  byte  swap  (1) 

0—0  2  jumper  If  you  want  to  select  word  swap  (1) 

o  o  3  there  should  never  be  a  jumper  on  these  pins 

o  o  4  jumper  If  you  are  using  a  IBM  RT  computer 
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21  20 
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9  8  7  6  5  4 


LO  o  X  X  X  X  S 


1  2  3  4  .0 
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VMEbus  INTERRUPTS  (0  Is 
the  status  error  Interrupt) 


o  o  o  o  o  o  o 
15  12  11  10  5  4  3 


PC-AT  INTERRUPTS 


nr 

»  ^  3  V 
o  o  0  o 

o  O  O  f> 


Lc> 

i  3  Z  I 

O  Q  d  o 
o  O  o  o 
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12.0  Appendix  D:  Safety  Light  Fence  Schematics 
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